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ABSTRACT 



A sliding mode compensator for depth control of an autonomous underwater 
vehicle (AUV) using depth feedback only is designed. The controller is evaluated 
for a nominal linear model and optimized by a series of numerical experiments 
for a number of depth changing maneuvers. A state observer is used in order to 
estimate the unmeasurable states together with the sliding mode controller. The 
effects of varying control parameters are discussed. Compensator performance 
is assessed by numerical simulation of AUV dynamic response based on the full 
six degrees of freedom nonlinear equations of motion. The expected robustness 
of the design is demonstrated by comparison between linear and nonlinear vehicle 
response characteristics, and by a wide variation in vehicle parameters and 
hydrodynamic coefficients. Finally, suggestions for design improvement and di- 
rections for future research are indicated. 
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1. INTRODUCTION 



A. GENERAL 

There has been an increased interest recently in the need for autonomous 
underwater vehicles (AUV) in both Navy and private industry. A variety of un- 
classified missions includes ASW, decoy, survey, reconnaissance, and ocean engi- 
neering work service. As the cost of manned submarine vehicles increases, there 
are significant advantages to the use of cheaper unmanned vehicles. The AUV 
should be able to maneuver freely in the ocean enviroment with respect to depth, 
heading, and speed in order to carry out its missions. Such maneuvering require- 
ments have to be easily accomplished by a low level active control system, and in 
the presence of environmental and physical uncertainty. 

All information concerning the environment of a vehicle is detected by the sens- 
ing level of control on-board the vehicle and directed to the high level intelligent 
system in order to carry out an unmanned mission. The dynamics of underwater 
vehicles are described by highly nonlinear systems with uncertain coefficients and 
disturbances that are difficult to measure. Robust control using variable structure 
systems are reputed to provide accurate control of nonlinear systems despite un- 
modeled system dynamics and disturbances, leading to the motion that sliding 
mode compensators should be employed in situations where accurate tracking is 
desired and where maneuvering parameters of AUV change with operating con- 
ditions. 

B. AIM OF THIS STUDY 

This thesis aims at investigation of the use of sliding mode compensator for 
AUV depth keeping and changing. The control concept developed here is that 
of a variable structure system consisting of continuous subsystems together with 
suitable switching logic. The sliding mode control concept was suggested by V. 
Utkin [Ref. 1] and recently developed by J.J.E. Slotinc [Ref. 2]. Because sliding 
mode control requires full state feedback, this work has incorporated a state ob- 
served based on output measurement resulting in a sliding mode compensator. 



The main goal of this thesis is to present a design procedure and estimate 
robustness of the variable structure compensator in the presence of vehicle non- 
linearities, modeling errors, uncertainties, and variation of parameters. 

C. THESIS OUTLINE 

Chapter 2 introduces the basic concept of Liapunov stability and an 
asymptotically stable condition which is related to energy degeneration with in- 
creasing time for a dynamics system. The other sections will discuss how to de- 
sign a sliding surface and a control law based on a linear model. The last section 
of the chapter presents a technique to eliminate chattering in order to provide 
smooth control inputs. 

In Chapter 3, vehicle dynamics and a process used to produce a linear state 
space representation are described. Sliding control law for a linear model is de- 
signed using results of the previous chapter. The design is evaluated through 
computer simulation. 

Chapter 4 presents the sliding mode compensator using depth measurements 
only’. A model with perturbed hv'drodvnamic and geometric parameters is used 
to estimate the performance of the sliding mode compensator. The chapter ends 
with a discussion of the robustness of the sliding mode compensator and advan- 
tage of the variable structure control system 

Finally. Chapter 5 contains a summary, conclusions, and some directions for 
further research. 



II. THEORETICAL BACKGROUND OF SLIDING MODE CONTROL 



A. GENERAL 

The dynamics of underwater vehicles are described by highly nonlinear, high 
order systems with uncertain models and disturbances that are difficult to model. 
A new form of sliding mode (Variable Structure System) control has been devel- 
oped recently, and shown to apply to a large class of nonlinear systems [Ref. 3]. 
Sliding mode control offers the control designer new possibilities for improving 
the quality of the control in comparison with a fixed structure system. The basic 
idea is to design a controller structure which consists of a set of continuous sub- 
systems together with suitable switching logic according to [Ref. 1]. The basic 
sliding mode control for a S1SO system 



x = Ax+Bu 



u = [ ± H'x] 



( 2 . 1 ) 



where. x is the state variable 

u is the sliding mode control law 
M y is a switched feedback gain 
A,B are system matrix. 

This chapter is devoted to the study of the basic background of the sliding 
mode theory for the design of a linear controller for the AUV. 

B. LIAPUNOV STABILITY 

For a given control system, stability is usually the most important thing to 
be determined. If the system is linear and time invariant, then, many stability 
criteria are available, such as the Nyquist stability criterion, the Routh's stability 
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criterion etc. The second method of Liapunov is the most general method for the 
determination of the stability of nonlinear and time varying systems. Before dis- 
cussing the sliding mode control, the second method of Liapunov will be dis- 
cussed in order to understand the sliding condition, which will be discussed in the 
next section. The basic concept of the second method of Liapunov is that if the 
system has an asymptotically stable equilibrium state, the stored energy of system 
decays with increasing time until it finally assumes it's minimum value. In order 
to explain this, Liapunov introduced the so called Liapunov function, an imagi- 
nary energy function which depends on the state variable (vi,jc ; ...v n ) and time (t). 
If the Liapunov function is denoted by V(x.t) and it's time derivatives denoted 
by 



r(„v,0 



dv(x.t) 

dt 



( 2 . 2 ) 



then the Liapunov function has information as to stability, asymptotic stability 
or instability of an equilibrium state of the system without solving the state 
equation. The theorem of the Liapunov function is described in modern control 
engineering [Ref. 4], If a system is described by 



x=J[x,t) (2.3) 

where x is the state variable and if 
f(0.t) = 0, for f 0 < t. 

and there exists a Liapunov function V(x,t) having continuous 1 and satis- 
fying the following conditions: 



1. V(x.t) is positive definite, and 

2. /'(.Y.r) is negative definite. 



4 



then the equilibrium state at the origin is asymptotically stable. I%x,t) is 
negative definite which shows that V(x,t) is continually decreasing. So for any 
system 



X =Ax,u( 0) 



(2.4) 



where, x is the state variable 
u(t) is the control law 
x(O.t) = v 0 



and the time derivative of the Liapunov function r(x,w(r)) is negative definite, 
then control law u(t) is guaranteed stable. 

C. DYNAMICS OF SYSTEM WITH SWITCHING 

Now consider the case that an asymptotically stable system may consist of 
two structures neither of which is asymptotically stable. If the differential 
equations of the second order system have the following format: 



dx 2 

~cU 



= Vx j 



(2.5) 



where u is a constant < | 4^ | 

then the structure of the system is elliptic as described in Figure 1 on page 6. 
Suppose the system with a positive feedback gain, then structure is aperiodically 
unstable as shown in Figure 1. The block diagram of the closed loop system with 
switching gain is illustrated in Figure 2 on page 7. Let us try to combine the 
advantage of both systems by suitable choice of their structures in the appropri- 
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Figure 1. Asymptotically unstable structure |Ref.5] 



ate parts of the phase plane. In order to get the asymptotically stable structures 
in Figure 3 (b), the phase plane was divided into four pairwise subsections as 
shown in Figure 3 (a) by the following conditions: 

1. Subsection 1 : a'j > 0, a 2 + v I'a,>0 

2. Subsection 11 : a, < 0, a 2 + v I'A| > 0 

3. Subsection 111 : a, < 0, a 2 +M'A| < 0 

4. Subsection IV : a, > 0, Aj+Ta, < 0 

The good phase trajectory for each phase portrait has been chosen to make 
asymptotically stable structures as in Figure 3 (b). 

This phase plane is separated from one another by the straight line a, =0 and 
a\+T.v,=0 which we call the switch line or sliding surface. The asymptote 
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Figure 2. The block diagram with switching gain 



a ' 2 + M ' a ',=0 acts as a switching line for the structure when the trajectory of the 
subsection 1 is reached. The structure of the system can be switched by using this 
line instantaneously from elliptic to hyperbolic in this case. This switching line is 
very important in sliding mode control. Once the system state trajectory ap- 
proaches the switching line, and in order to keep the trajectory on the sliding 
surface for t > t 0 , then this system will become asymptotically stable as long as it 
satisfies the Liapunov condition. In the general case, a switching line might be a 
straight line, a = (0 < X < oo), but must pass through the origin (i e 

*( 0 ) = 0 ). 

D. SLIDING CONDITION 

Some of the possible advantages offered by the idea of switching the structure 
of a control system were described in the last subsection. But we remarked that 
if the structure does not change at the precise instance when the trajectory crosses 
the switching line, due to the effect noise, then additional control action will be 
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Figure 3. Phase plane division and combined stable structure (Ref. 5) 

required to enforce the sliding condition. The motion of the system now depends 
on sliding surface parameters which are insensitive to the external disturbances 
and variations of the plant parameters within a wide range of the switching line. 
If the system trajectory of the subsection I crosses into the switching line and af- 
ter passing the system trajectory of the subsection II crosses over the switching 
line repeatedly again, the system trajectory will be kept within some range of the 
switching line. Suppose such change occurs at infinitely high frequency, then the 
state of the system trajectory is maintained with an infinitesimal amplitude oscil- 
lation. It is an asymptotically stable system on this line as shown in Figure 4 on 
page 9. 

The motion of the system on the switching line is described by the solution 
of the general differential equation (2.5) [Ref. 2] together with the equation of the 
general switching line given by 
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Figure 4. Switching line and asymptotic trajectory [Ref.Sj 



= ( ~aF = 0 I 2 - 6 ) 

where x = x—x d , a single element of the state vector 
?. > 0 ( arbitrary constant) 
n = order of system 

The motion defined by the equation (2.6) describes the system dynamics in the 
sliding mode. A sliding mode has an important property that the corresponding 
motion of the system depends on the sliding surface (Switching Line) which is 
chosen by the designer only. In order to know the mathematical existence condi- 
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tion for the sliding mode, the theory of sliding mode has given a considerable at- 
tention to the methods guaranteeing the existence of the sliding mode. 

1. Condition for Existence of a Sliding Mode 
Considering the general dynamic system 



(2-7) 

Let us assume that the right hand members of this equation are discontinuous 
on a certain sliding surface c(a') = 0 in the phase space, where, as phase trajectory 
of the system approaches a(.v) = 0 from either side, the following limits are de- 
fined [Ref. 5]: 



\\m f{xj) =y~( x.t) 

<7 — * 0 



lim fix.,)=/ + (x,l) (2.8) 

<7 — * 0 * 



where f-{xj)^f~{x,t) 

then the derivative of the sliding surface (a) along the trajectories of the system 
is 



da 

dt 



rg dx 
CX dt 



f= if -grade) 



where f = phase velocity vector, and 



(2.9) 



lim_— r- = (T". grade) 
<7—* o at 
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lim = {/'grade) 

< 7 ^ 0 “ CU 



( 2 . 10 ) 



where o,f is a smooth function. 

At each point of a = 0, the sign of the limit equation (2.8) has seven cases. There 
is a case which is most interesting as it corresponds to the existence of an ideal 
sliding mode on the sliding surface o = 0, if 

lim -^<0 
a— 0~ dt 

lim ^>0 (2.11) 

cr— » 0 _ dt 

The equivalent inequality being the condition for existence of a sliding mode is 

lim o < 0 
c — * o at 



or 



d(c r ) 

lim — — <0 2.12 

(T-o dt 



2. Proof of Stability 

This inequality is also suggested by [Ref. 5] as a necessary condition for 
the system in equation (2.5) to have a degenerating Liapunov function in the 
following form: 



r(.v) = y 0(x)] : 



(2.13) 
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The asymptotic stability of the system (2.5) is guaranteed provided that l'(x) is 
negative definite as discussed in the previous section. If cr(x.t) is a sliding surface 
by the definition above, it follows that 

y -y A-M) £ ->f I e(x,l) I (2.14) 

where ?/ is the sliding control gain and 
is the sliding surface. 

It will guarantee stability of the sliding mode motion. The control law driving 
motion in the sliding mode can be obtained by using this condition [Ref. 2]. The 
next section will be devoted to a discussion for the development of the sliding 
mode control law. 

E. SLIDING SURFACE DESIGN 

The sliding surface has a very important property that is shown in the previ- 
ous section. The sliding surface should be designed so that system response re- 
stricted to <7(.v) has a desired behavior, such as asymptotically stable state or 
tracking error. 

Let us consider a linear time invariant system to design the sliding surface 

a = Ax+Bit (2.15) 

where a e R n , u e R m 

Consider that the sliding surface of the equation (2.15) has the following form: 

<7 (a) = S^x — 0 (2.16) 



where S is the sliding surface coefficient (rn x /;) 



The existence of the sliding mode implies that a(x)a(x) < 0 and a(x) = 0 for all 
t > t 0 . Using the method of equivalent control [Ref. 6] 



a( x ) = 0 = 4r~-^ L = S T x = 0 (2.17) 

C X at 

Substituting equation (2.15) for x of the above equation 

S T (Ax+Bu eq ) = 0 

or 

u eq = ~[S T BY { S T Ax (2.18) 



Substituting equation (2.18) to Eq (2.15) and rearranging 

x = /Ix-RCS^r'^/lx 



or 



x = [.-1 — 5( S 7 R ) - 1 S ]x (2.19) 

Equation (2.19) gives the dynamics of the system on the sliding surface for t > t 0 
given <r(x) — 0. but the S matrix is unknown. In order to determine S matrix, the 
equation (2.19) can be rearranged in the following form: 

x = [A — BK c ~]x 



x = A c x (2.20) 

where A' c = (S r R)-'S 7 .-l 
A c = A—BK C 

The K c matrix can be obtained from the pole placement for which we can select 
specifically desired closed-loop poles of the system equation (2.20) on the sliding 
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surface. If we get the K c matrix by using the standard pole placement method, the 
sliding surface matrix (m x n) can be determined in the following procedure: 

K c = (S T B)~ } S T A 
S t A-S t BK c = 0 

S t {A—BK c ) = S t A c = 0 (2.21) 

It should be noted that A c must be rank deficient by one and that the procedure 
must therefore place one pole of A c at the origin. The left eigenvector of the A c 
matrix of equation (2.21) corresponding to a pole placed at the origin are the 
sliding surface coefficients which give the desired behavior system on the sliding 
surface. 

F. SLIDING MODE CONTROL LAW 

Given the dynamic model, the sliding surface definition, and the stability 
criteria, a suitable control law can be obtained. We assume that a wide range of 
single input, single output dynamic systems, and sliding surfaces can be described 
by 



x = Ax+Ba 



( 2 . 22 ) 



(t(av) = S T x(t) 

where S T is a row vector of the form [1 , 5 2 . 5 3 .... 5 n _,], a specific choice of S T to 
achieve a stable tracking error and to enhance robustness as discussed in the 
previous section. If u(t) could be chosen so as to keep the trajectory on 
c-(.Y.r) = 0, we would have the sliding control law from the sliding condition 
equation (2.14) and sliding surface equation (2.16) that 
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T 2 

&(x) = S x = —rj sign(o ) 



S T A+S T Bu = —}]Qsign(a) 

u = -(. S T B)~ X S T Ax-{S T B)~ X nlsign{a ) (2.23) 



where ?/g is a arbitrary nonlinear feedback gain 
The control law has two parts. 



u = u+u 

where it is linear feedback control law 
u is nonlinear feedback control law 

Initially, it compensates directly for the known portions of the dynamics. Thus. 
It is discontinuous across the sliding surface. This nonlinear term is obtained di- 
rectly from the time-varying bounds on parametric uncertainty and disturbances. 
As a result, control discontinuity across a - 0 grows as the model becomes less 
certain and increasingly disturbed. This insures that a 1 is a Liapunov function 
of the closed-loop system, since it satisfies the sliding condition equation (2.14) 
and thus guarantees stability despite the uncertainty in the model and disturb- 
ances [Ref. 2j. This type of control law can guarantee stability and perfect 
tracking for a large class of nonlinear systems. The discontinuous form results in 
a chattering type of control action that would be very undesirable for most sys- 
tems and this chattering behavior has been one of the main reasons sliding con- 
trol techniques have not been more widely applied. This problem will be solved 
by smoothing out the control law in a thin boundary layer around the sliding 
surface as given in the next section. 
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G. CONSIDERING CHATTERING AND UNCERTAINTY 

While sliding mode control provides a control law which is robust to param- 
eter variations and disturbance inputs, it was a chattering problem for the input 
as shown in Figure 5 on page 17. In fact, imperfections such as delays in 
switching and hysteresis in switching, will cause the trajectory to chatter along the 
sliding surface. Although as such imperfections vanish, control activity remains 
as undesirable switching and high frequency signals on the sliding surface. 

The basic idea of eliminating chattering is simple. This chattering problem 
caused by a discontinuous and nonlinear switching feedback control law can be 
eliminated by replacing it with a continuous feedback control law. But if the 
sliding mode control has a continuous feedback function, there are steady state 
errors due to variations in parameter and disturbance [Ref. 2]. Suppose the con- 
trol law has a continuous feedback, whose terms are continuous function inside 
a small boundary layer thickness on the sliding surface, as shown in Figure 6 on 
page 18, then the steady state error can be calculated by a smooth function which 
eliminates chattering in the boundary layer thickness ((/>). This boundary layer 
thickness can be determined directly from the desired sliding surface coefficient 
limit and estimates of the uncertainty dynamics of the system to be controller. If 
the specified bounds on disturbances and parameter uncertainty are not ex- 
ceeded. the system is guaranteed to stay within the boundary layer once inside. 
If a disturbance temporarily exceeds the specified bounds, the state may go out- 
side the boundary layer. However, the sliding condition equation(2.14) implies 
that the system will always move back inside the boundary layer once the dis- 
turbances return to their projected levels. 

The dynamics of the state trajectory inside the boundary are only an ap- 
proximation to the desired dynamics on the sliding surface. The advantage of the 
scheme is that the state trajectory docs not chatter close to the sliding surface. 
To carry out the preceding program, we use the sliding surface considered in the 
prev ious section with o-(.v.r) of the form: 

<?(x) = A i x(r) (2.24) 
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Figure 5. Chattering problem 

where x(i) = x(t)-x\,(t) 

Xj(i) = desired state variable 

To define the boundary layer thickness about the sliding surface of equation 
(2.14), define 

<7 + (a) = o{x)+({) 

o~(x) = o{x)-4> (2.25) 

where 4‘ > s die boundary layer thickness 



It is immediately from equation (2.25) that 
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| a{x) | < 4> (2.26) 

and 

~jp = -jj- o{x,t) = o + (x,t) (2.27) 

We choose the control law »(/) as by equation (2.27) for a~{x,r ) < 0 or 
ct(aV) > 0. This guarantees that 

-jo~(x,t)> 0 (2.28) 

-£■ < 0 (2.29) 
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Equation (2.28) and (2.29) establish that trajectories starting outside boundary 
layer tend towards boundary layer, and further trajectories starting inside 
boundary layer stay in it for t 0 < t. It only remains to specify u(x,t) to be a con- 
tinuous function of x inside boundary layer thickness. 
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III. SLIDING MODE CONTROL FOR NONLINEAR A.U.V. IN DIVE 

PLANE 



A. GENERAL 

Underwater vehicles present difficult control system design problems due to 
their nonlinear dynamics, uncertain hydrodynamic coefficients and the presence 
of disturbances that are difficult to measure or estimate. This chapter describes 
the dynamics of a selected automonous underwater vehicle (AUV) and the design 
of a sliding mode controller which can handle these problems effectively. 

Motions of underwater vehicles are expressed in a body fixed reference frame, 
because hydrodynamic forces and inertia properties are most readily computed 
in a ship reference frame. The nonlinear equations of motion in six degrees of 
freedom vehicles which are commonly known as the DTNSRDC 2510 equations 
of motion are used for verification of the sliding mode control design. These 
highly nonlinear equations of motion are linearized by a Taylor series expansion 
and modified to suit the needs of an AUV [Ref. 7], First, these linearized and 
modified equations of the system are used to design the sliding surface, sliding 
mode control law and observer. These values of the linear system are, then, used 
to implement the sliding mode control law for the nonlinear system. This chapter 
shows how to design the sliding mode control for the highly nonlinear AUV. 

B. NONLINEAR COMPUTER MODEL 

The nonlinear model used for sliding mode control verification was derived 
from the original NSRDC 2510 document [Ref. 8]. The nonlinear model used in 
this thesis consists of 8 differential equations which describe the AUV dynamics. 
The six equations as derived from force and moment equalities account for the 
states u, v. w. p, q and r. The shape of AUV, which is 17.4 feet long, weighs 
12000 pounds, and neutrally buoyant, is depicted in Figure 7 on page 21. 
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General dynamical equations are derived from Newton's law in an inertial 
reference frame: 




= /[ dynamical response terms ) 



(3.1) 



The general form of the force balance is 






(3.2) 



where M = mass matrix 

.V = [ u, v, w ] r 

and the moment balance is 



.f/ = 7(/- Q ) (-3-3) 

where I = moment of inertia matrix 

Q=[/M h rY 

Three dimensional motions of underwater vehicles are normally described using 
the body-fixed coordinate and inertial reference frame. Position of the body-fixed 
coordinate system is expressed in X, Y. and Z coordinates and orientation of the 
vehicle's coordinate system in expressed in Euler angles (f). 6 , and \}/. The defi- 
nitions of u. v. w. p. q. r and controls are listed in Table 1 and Table 2. 






Table 1. DEFINITION OF A U V STATES. 



STATE 


DEFINITION 


UNITS 


u 


surge rate 


(ft s) 


V 


sway rate 


(ft s) 


\v 


heave rate 


(Ft, s) 


p 


roll rate 


(rad s) 


q 


pitch rate 


(rad s) 


r 


yaw rate 


(rad/s) 


</> 


spin 


(radians) 


e 


elevator 


(radians) 




azimuth 


(radians) 



Table 2. DEFINITION OF A.U.V CONTROLS 



CONTROL 


DEFINITION 


br 


rudder angle 


Sts 


starboard bow plane angle 


<5 bp 


port bow plane angle 


ds 


stern plane angle 




delta form 


<5* 


delta buoyancy 



The dynamical response terms of the left hand side of equation (3.1) or 
equation (3.2) and (3.3) express the external forces and moments exerted on the 
vehicle by hydrodynamic, control surface, propulsion and other effects. The force 
and moment equalities of equation (3.2) and (3.3) describe motions in six degrees 
of freedom of the AUV. The three forces are in the axial, lateral and normal di- 
rections which give rise to motions in surge, sway and heave respectively. The 
three moment equations produce moments and motions in roll, pitch and yaw. 
Figure 8 on page 24 shows the positive directions of forces, moments, motions, 
and control surface deflections. 
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The equations of motion for the six degrees of freedom for the fully nonlinear 
model are listed in the following page. The hydrodynamics coefficient of those 
equation used for this thesis are those that were determine! using an analytic ap- 
proach [Ref. 7] and later simplified [Ref. 9]. The four nonlinear equations that 
are considered for designing sliding mode control of the AUV are written in the 
following form: 

Normal Equation of Motion 



m[\v-u x q+\'p+X g {pr-q)+ Y g {qr+p)-Z g (p 2 +q 2 )] = 
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4" l\Z' q q+Z' p /+Z pr pr+Z' n y 2 ^ 



+ -y l'[Z' W w+Z' q u x q+Z' vp vp+Z' VT vr] 



+ 4" ^[■Z' M .w A; vv+Z'w v ‘'+Wx(Z' ( 5 5 5 5 +Z'^5 fe )] 



P_ 

0 



[C Dj: //(a-)(v+at) 2 +C d ^(x)(w-x^) 2 ] U \ 4r </a ‘ 

^ c/V*/ 



+{\V-B) cos 0 cos (p 



+ "T ,; 2', f l!>z \v n ","'+ z 'i„ J "?\,) ! -<’!) 



(3.4) 



Pitch Equation of Motion 



h4+{lx-Iz)P r ~ l . yz(P ( l->')+ I xz(p 2 - , ' 2 )- m l x , 'Jw-u x q+vp)-Z (w-vr+vny)] = 



'>’2 



P 



— /' [.V'^+.U'^/r+.U'/p/yjr+.A/'m- 2 ] 



+ 4 /"DU^^+.l/V^+.'V^+^^vr^ 



+ 4- / 3 [^\,/^n-+,A/' vv v 2 + W 2 (A/'^,+A/' 66( 5 6 )] 



P 



[C 0t ./?(A)(r+AT) 2 + C D J>(.\-)('r-,Y< 7 ) 2 ] h . 4, 1 - v</ - v 

c cy\ A J 



— (A^IF— X b B) cos 0 cos <j)—(Z g lV—Z B B ) sin 0 



+ y y (3-5) 

Kinematic Relations 



0 = q cos <fi—r sin <j> (3.6) 

Z = -u x sin 0+v cos d sin </>+vr cos 6 cos 4> (3.7) 

The simulations for the di\ e plane control were performed by using the 
FORTRAN language code (Appendix. B) for the simulation of nonlinear system 
response as a function of time. 

C. LINEAR MODELING 
1. Equations of Motion 

The sliding mode controller design procedure begins with the expression 
of the equations of motion in linear time invariant state space form. The highly 
nonlinear AUV system is 

-jL x(t) = "(')) (3-8) 

no = swo) (3.9) 

where x is the state vector 

u is the control input vector 
v is the output vector 

Although equations (3.8) can be significantly simplified as in Larsen [Ref. 9]. they 
appear still very complex for this study. The nonlinear equations can be linearized 
through a Taylor series expansion in the vicinity of a nominal point (ideally where 
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- 7 - (/) = 0) for small deviations of u(t) and x(t) from the reference values. These 
at 

equations of motion were linearized by Boncal [Ref. 7]. Alternatively, least 
squares techniques can be used for parameter identification in order to develop 
a linear model for the relationship between dive plane angle (< 5 ). pitch rate (q), 
depth (z) and pitch angle (6) [Ref. 10]. The linearized and very simplified 
equations for the dive plane motion 

q = — 0.1 q— 0.036— 0.02>58 



0 = q 

z = -u 0 e (3.io) 

were found to provide a satisfactory approximation of the open loop dive plane 
dynamics of nonlinear equation (3.4) to (3.7) for the nominal speed of 6 ft/sec (or 
500 rpm ). 

2. Sliding Surface Design 

It is evident from the discussion of the sliding mode theory (Chapter 2) 
that description of AUV motion depends on the sliding surface regardless of dis- 
turbance and unmodel parameters, after it hit sliding surface. So it is very im- 
portant to design sliding surface of the AUV with disturbance and unmodellcd 
parameters. The sliding surface of the AUV will be designed based on the linear 
model. The state space form of the linear model is 

x = Ax+Bu 

y = Cx (3.ii) 



where x T = [<y. 6. z] 



A = 



-0.7 

1 

0 



-0.03 0 
0 0 
-i’o 0 
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-0.035 



B = 



0 

0 



and 



u — d 

C= [00 1] 



For a three dimensional system, the sliding surface is the Euclidean plane 
o(x) = S T x = 0 



where 5 = 



Equation (2.6) can be expressed in terms of the state variables and sliding surface 
coefficients as following 



o („y) = q+s 2 0+si,z (3.12) 

where the coefficient of q has been normalized to 1. S will determine the sliding 
surface plane uniquely. To compute the equivalent control ( u eq ). we substitute A, 
B and S in to equation (3.11). and the closed-loop dynamics of the linear model 
are 



x = [A-B{S T Bf l S T A^\x 



or 



x — ( A — BK)x 



(3.13) 



2S 



where the gain vector K can be found from standard pole placement methods. 
The closed-loop dynamics matrix 



A c = A-BK 



(3.14) 



where K = (S T B)-'S T A 



has eigenvalues specified for desirable response of the AUV. One of the 
eigenvalues of A c must be specified as zero. With A c specified and K computed 
from pole placement, S T matrix can be determined as using the equation (3.13) 
and (3.14). 



Therefore S T is found as a left annihilator of A c or S is a left eigenvector of A c 
which corresponds to the zero value. This sliding surface of the AUV satisfy the 
sliding surface condition S T x = 0. To find proper sliding surface of the AUV, 
Matrix-x program "SCM" in Appendix A is used. The response of the AUV ac- 
cording to different sliding surface is shown in Figure 9 on page 30, Figure 10 
on page 31, and Figure 11 on page 32. The sliding surface of Figure 10 which 
has no overshot and fast response time is selected as the reference sliding surface 
of the AUV in order to design control law. The sliding surface of the AUV is 



This sliding surface has the desired dynamics of the closed-loop system. The 
perfect depth tracking of the selected AUV is then defined as remaining along the 
surface. The dynamic response of AUV is affected by the chosen sliding surface. 
The selected sliding surface is used to handle accurate depth control. 



S T {A-BK)=0 



or 




(3.15) 



cr(v) = <7+0.52(1—0.01 12z 



(3.16) 




Figure 9 . The dynamic response for the closed-loop poles = 0, -0. 1,-0. 15 
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o 




in 




TIME ( SEC ) 



Figure 10. The dynamic response for the closed-loop poles = 0. -0.25.-0.27 
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o 






Figure 1 1. T he dynamic response for the closed-loop poles = 0,-0. 45, -0.47 
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3. Sliding Mode Control Law 

Here the goal is to determine switched feedback gains which will drive the 
AUV state trajectory to the sliding surface and maintain sliding mode conditions. 
By defining the Liapunov function of the equation (2.13) 

= f (3.17) 

where o{x) = <7+0.520— 0.01 12z 

asymptotic stability of the AUV on the sliding surface is guaranteed, that pro- 
vided I 7 (x) is a negative definite function or 

o{x)6{x) = -if | c(x) | 



or 



6(x) = —r} 2 sign(c(x)) 



(3.18) 



Since o{x) = S T x , we have 

S T (Ax+Bu) = —ifsign(a(x)) (3.19) 

and solving for the equivalent control input u 

u = ~(S T B)~ ] S T Ax—(S T B)~ l i] 2 sign(c) (3.20) 

It is important to recognize that the feedback control law is composed of two 
parts. 

u = u+u 

The first it = — (S T B)~ i S T Ax is a linear feedback control law, where the second 
u = —{S T B)~h] 2 sign(a(x)) is a nonlinear feedback with its sign toggling between 
plus and minus according to which side of the sliding plane the AUV is located 
in. Two comments are in order here: First, since u has to change its sign as the 
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AUV crosses a(.v) = 0, the sliding surface has to be a hypcrplanc (dimension of 
one less than the state variable). Second, it is u which is mainly responsible for 
driving and keeping the AUV onto the sliding plane a(x) = 0 (where u = 0 as 
well). The block diagram of the sliding mode control is illustrated in Figure 12 
on page 34. 




Figure 12. The block diagram of the sliding mode control 



Provided that the gain has been chosen large enough, u can provide the 
required robustness due to momentary disturbance and unmodcled AUV without 
any compromise in stability. The linear feedback law is designed such that the 
AUV has the desired dynamics on the sliding plane. Since <t(.x) = 0 in this case 
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u = u = -(S T Bf's T Ax 



(3.21) 



Large enough gain (ETA = =4.0) could be used in the computer simu- 

lation by using Matrix-x and FORTRAN program "SMC" in Appendix A. The 
dynamic response of the AUV according to several values of gain is shown in 
Figure 13 on page 37, Figure 14 on page 38, and Figure 15 on page 39. Choos- 
ing properly large enough nonlinear feedback gain, although the response time 
of the AUV is fast, the AUV overshoots on the sliding surface as shown in 
Figure 15 on page 39. The dive plane angle of the AUV presents chattering due 
to sign switching of the nonlinear part of the control law. Several varieties of 
nonlinear feedback gain (;/ ; ) and boundary layer (</>) are evaluated through a se- 
ries of numerical experiments in order to analysia dynamic response of AUV 
which are dependant on it. The characteristic of dynamic response according to 
the nonlinear feedback gain, boundary layer, and closed-loop poles are shown in 
Table 3. 



Table 3. THE DYNAMIC CHARACTERISTIC OF THE AUV ACCORDING 



TO DESIGN PARAMETER s 



Parameters 


Depth re- 
sponse 


Div e plane 
angle 


Pitch angle 
(radians) 


Sliding sur- 
face 


C.P = 

0, -0.1. -0.1 5 


slow, no 
overshoot 


no saturated 


maxi 

angle = 0.4 


no overshoot 


C.P = 

0. -0.25. -0.27 


fast, no oxer- 
shoot 


no saturated 


maxi 

angle =0.4 1 


no overshoot 


C.P = 

0.-0. 45.-0. 47 


fast, over- 
shoot 


saturated 


maxi 

angle = 0.42 


overshoot 


ETA = 3. 
PHI =0.2 


slow, over- 
shoot 


no saturated 


maxi 

angle = 0.4 


overshoot 


ETA = 4. 
PHI =0.4 


fast, no over- 
shoot 


no saturated 


maxi 

angle =0.41 


no overshoot 


ETA = 8. 
PHI =0.4 


fast, no ov er- 
shoot 


saturated 


maxi 

angle =0.42 


overshoot 
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We already knew that system behavior is dependent on the sliding surface which 
is designed based on closed-loop poles. The closed-loop poles (0.-0. 25,-0. 27), 
nonlinear feedback gain (ETA = 4), and the boundary layer (</> = 0.4) are selected 
in order to satisfy the necessary motions of the AUV. 
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Figure 13. The dynamic response for ETA(3) and PH1(0.2) 
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Figure 14. 



The dynamic response for ETA(4) and PM 1(0.4) 





O LT> 




Figure 15. The dynamic response for E I A(S) and PH 1(0.4) 
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4. Chattering Problem and Steady State Error 

The nonlinear part of the sliding mode control may give rise to chattering 
where constant or random disturbances are present, as shown in Figure 16. 




Figure 16. The chattering problem of sliding mode control 

A choice of a saturation function (satsgn(a)) instead of the pure switch (sign(a)) 
for the nonlinear control law is prefered in order to avoid chattering problems. 
Since the boundary layer thickness is related to the characteristic of dynamic re- 
sponse, it can be selected according to the manuvering conditions of underwater 
vehicles. Therefore the control law for the AUV is modified to 

u = —5. 1429^+ 1.07 140+ K„satsgn(a) (3.22) 

where K n = i] 2 (S 7 B)~' 



and 
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+ 1 if a > f 

satsgn(o ) = — 1 if a < —<$> 



— if -<j> < o < f 
<P 



This provides a linear interpolation across the boundary layer as illustrated in 
Figure 17 on page 42. 

A boundary layer thickness (<£) essentially assigns a lowpass filter structure to the 
dynamics of the sliding surface (<j) . If a specific bound on disturbance is not ex- 
ceeded, the system is guaranteed to stay within the boundary layer once inside. 
The steady state error of the sliding mode control of the AUV with a disturbance 
can be computed as following. From the equation (3.1 1) 



q = -0Jq-0.03d-0.035u+d 



(3.24) 



where d is disturbance 



q = q = 0 

0 — 0 in steady state 



The steady state control input for the given disturbance is 



d 

ss 0.035 



(3.25) 



The control law in steady state is 




(3.26) 



where cr(.v) = —0.01 12<?^ 
f- = ~ r --c 
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Figure 17. The saturation function for the nonlinear control lan. 



« 1 55 = '/ ( 



2 —0.01 1 2 x e., 



</> 



» I55 x 4 > 
0.01 12 x }) 2 



(3.27) 



or 



d x </> 

0.01 12 x ?/ 2 x 0.035 



(3.28) 



The steady state error can be minimized by increasing the nonlinear control gain 
(>/ : ) and decreasing boundary layer (</>) as shown in Figure 18 on page 44. How- 
ever, it can never be completely eliminated. The depth response of the Figure 18 
(a) show the steady state error for the constant disturbance (d =0.005) when us- 
ing the nonlinear feedback gain (ETA =4). Large nonlinear feedback gain is used 
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in order to decrease the steady state error as shown in Figure 18 (b). The steady 
state error for the command depth is decreased down to 10% approximately us- 
ing large gain. However, dive plane is kept the same angle regardless nonlinear 
feedback gain. If we use large gain and small boundary layer, the depth response 
of the AUV may overshoot and the dive plane input may have numerical chat- 
tering. This is different from the chattering due to disturbance and unmodeled 
dynamics as shown in Figure 19 on page 45. This numerical chattering problem 
is an artifact of the numerical integration method (a fixed step Kutta-Merson and 
fixed Euler's method were used in the simulation) and, in principle, it can be re- 
duced by choosing a more accurate method with smaller time step. 
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(a) 



O 





(b) 



Figure 18. The steady state error for increasing nonlinear feedback gain 
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Figure 19. Hie numerical chattering problem 
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5. Robustness tests 

Another important aspect of the sliding mode control is its robustness to 
parameter variations and unmodeled dynamics. Sliding mode controllers are ex- 
pected to outperform more conventional linear state feedback with respect to 
robustness. This is true for the following two reasons: First, choosing if suffi- 
ciently large will guarantee that the system approaches the sliding surface 
asymptotically even for the off-design case. Second, since at the final stage the 
system evolves in a lower dimension state space (the sliding surface), it is na- 
turally more robust than the original higher dimensionality system. To verify 
these expectations, we changed the hydrodynamic coefficient of the AUV, AUV 
rpm and dive plane strength as in Table 4. The standard design AUV equation 
are designated by A, B matrices at 6 ft/sec (rpm = 500). 



Table 4. ROBUSTNESS TEST CASES 



TEST. NO 


A MATRIX 


B MATRIX 


SPEED(RPM) 


Nominal test 


A 


B 


6 ft sec (500) 


Test 1 


2* A 


B 


6 ft sec (500) 


Test 2 


A 


2*B 


6ft sec (500) 


Test 3 


A 2 


B 


6 ft sec (500) 


Test 4 


A 


B 2 


6 ft sec (500) 


Test 5 


2* A 


2*B 


6 ft sec (500) 


Test 6 


A 2 


B 2 


6 ft sec (500) 


Test 7 


A 


B 


12 ft sec (1000) 



The sliding mode control for the AUV was designed based on the nominal case 
as in the previous section. The nominal nonlinear feedback gain (if = 2.4) and 
boundary layer thickness (</> = 0.4) were applied to the modified system. The dy- 
namic response of the modified AUV is shown in Figures 14, and 20, to 26. The 
A matrix of test 1 model, which is related to the rotary damping coefficient, was 
doubled in magnitude in order to test the sliding mode control. Only the response 
time of test 1 model was longer. The test 3 model, which has a matrix equal to 
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one-half of the A matrix, presents fast response and overshoot due to decrease in 
the the rotary damping moment as shown in Figure 22. The hydrodynamic coef- 
ficient for the dive plane, which in proportional to dynamic response time, was 
changed in order to test performance of variable structure system for the AUV. 
In general, it is evident from the simulation results, that even under a 200% 
change in the coefficient of the A, B and RPM, the vehicle response remains 
stable, 7 ~ 8 % overshoot, faster or slower as expected. 
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Figure 20. The response of 2*A, B and 500 rpm 
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Figure 21. Hie response of A, 2 A B and 500 rprn 
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Figure 22. The response of A/2, B and 500 rpm 
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Figure 23. 7 lie response of A, B/2 and 500 rpm 
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Figure 24. The response of 2*A, 2*B and 501) rpm 
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Figure 25. The response of A/2, B/2 and 500 rpm 



53 







Figure 26. The response of A, B and 1000 rpm 
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D. DESIGN OF SLIDING MODE CONTROL FOR THE 
NONLINEAR MODEL 

The sliding mode appeal is based on its ability to treat nonlinear, time varying 
and unmodelled systems in a straightforward manner [Ref. 3], It is the purpose 
of this section to apply the linear sliding mode controller to the nonlinear model 
of the actual AUV. The sliding surface and control law gain for the actual AUV 
are based on the linearized nominal model. The mathematical form of the non- 
linear model for the actual AUV is 

“T =^A'T4/l.v)+5(.v)r/+rf(x, t) (3.29) 

where B(x ) is the nonlinear function associated with the control 
surface and actuator system 

A/(.v) is the uncertainty of the nonlinear function f(x). 
d{x. r ) is the uncertainty disturbance. 

The system matrix A is the estimate of f(x) and the magnitude of the uncertainty 
is bounded as 



F> I S r Ay(.v) 



(3.30) 



where F = 



F- 



F„ 



The individual bounds on any element of A/ as estimated from some knowledge 
of the extremes of possibility of AJ{x). Also, let B(x) be approximated by B. a 
constant, where the varying gain ft is defined by B= ftB(x). and ft is taken to be 
scalars, but bounded within the limits of ft mn < ft < ft max . and ft nomna , — 1 . Then it 



follows that /? = 1 , and Af = 0, and d(x,t)=0 will yield the nominal sliding mode 
control law (3.20). Disturbance d(x.t) is unknown but is upper bounded by a 
known continuous function such that: 

D > | S T d(x, t) | (3.31) 

The dynamics of the system with bounded uncertainty of the actual AUV is 

= Ax+AJ[x)+pBu+d(x, t ) (3.32) 



This equations is used to simulate the uncertain nonlinear terms after the 
linearization process. We have designed the sliding surface <t(jc) = S T x by using 
the nominal linearized model in the previous section. Using the sliding condition 
theory for the actual AUV, the sliding mode control law for the nonlinear model 
can be chosen from equations (3.17) and (3.18) such that if 

a(A')(7(.v) = - i ] 2 1 ct(a-) | 



and the system will reach the surface (a(v) = 0) within a finite time t r defined by 



t r < 



g(0) I 

>r 



(3.33) 



The true dynamics of the sliding surface with uncertainty arc, however, given by 

<7 (a*) = S 1 x 

= S T [Ax+Aj{x)+(JBu+d{x. /)] (3.34) 



Substituting equation (3.20) to the above equation for u, then the derivatives of 

<7, 



a = (\—p~ l )S T Ax+S T Af\x)+S T d(x, t) 



-P ] tfsign(o) 



(3.35) 
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From the above, stability is guaranteed, if and only if 



<*M £ ->ioS7g«(T), 



(3.36) 



where t ] 5 is the nonlinear feedback gain without an uncertainty 



If the control matrix B is exactly known (/? = 1), then control law 



u b = -[S T BY l S T Ax-lS r BY l >i 2 sign{a(x)) 



(3.37) 



where if > | if+F(x)+D{x, t ) 



can guarantee stability and perfect tracking for the nonlinear system with con- 
stant control matrix of the AUV. In case where the control system B(x) is un- 
certain. the following change (if) must be made: 



Since the nonlinear term of the B(x) and A/(.v) is uncertain in most cases, it is 
assumed to be zero equation (3.29) and if is increased depending on their as- 
sumed bonuds to guarantee stable sliding mode control [Ref. 2]. The actual 
control law used in subsequent simulation was in fact equation (3.38) with suffi- 
ciently large if to accomodate the uncertainty in A/(x) and B(x). The sliding 
mode switching control law equation (3.38) guarantees that equation (3.18) is 
satisfied even in the presence of parameter variations and unmodeled dynamics 
provided if is large enough. The dive plane angle chattering due to modeling er- 
rors and disturbances can be eliminated by defining a boundary layer thickness 



u — — [5 r £] 1 S T Ax—[_S T B'] ] rfsigno(x ) 



(3.38) 



u = u+u 



where u = ~[S T BY ] S T Ax 

u = — [S T BY'ifsign{c) 

>r > /U I m + f{x)+D(x, t) | + 1 (/U-i) 1 1 s^Ax 
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<f) about a = 0 as illustrated in the previous subsection. The smooth sliding mode 
control law of the actual AUV is then 

u = — \_S T B^\~ i ^S T Ax+}j 2 satsign( ■— (3.39) 

where ;/ : > /? max | ?/ 5 +/'(x)+Z?(x, 0 | + | (/J max - 1 ) | | S T Ax \ 

The dynamic response, dive plane angle, and sliding surface obtained from non- 
linear model simulation at 500 rpm are shown in Figure 27 on page 59. Al- 
though the sliding surface and control law gain based on the nominal linear 
system were applied to the nonlinear system, the response of the system is satis- 
factory as expected. The designed variable structure system based on the nominal 
linear equations dealt with the full nonlinear dynamics of the AUV as shown in 
Figure 27. The technique of sliding mode control can handle the nonlinear sys- 
tem directly without linearization, if sliding surface coefficients are properly cho- 
sen. This is especially important for highly maneuverable underwater vehicles 
that can move in all directions. 



o 





TIME (SEC) 



LD 





figure 27. The response of the actual AIJV 



E. ROBUSTNESS TESTS USING THE NONLINEAR MODEL 



The variable structure systems have an important property which is much less 
sensitive to nonlinearities, disturbances, and unmodeled dynamics as illustrated 
in the previous chapter. The nonlinear model of the AUV is used to demonstrate 
the properties of this control methodology. Accurate models are normally re- 
quired in order to achieve good control. However, since hydrodynamic modeling 
is a key element in the design of control system for AUV, we have to rely on ap- 
proximate expressions for the hydrodynamic forces which can introduce large er- 
rors into the control process. We have already seen the results of simulation for 
control performance using an accurate linear model, modified linear model and 
accurate nonlinear model. In this section, a simulation study is performed to il- 
lustrate the effectiveness of the sliding control under large modifications in the 
nonlinear model parameters. The modified hydrodynamic coefficients in Table 
4 are closely related to system stability. The rotary damping coefficient (M q ) af- 
fects the hydrodynamic moment of the AUV in the vertical plane. The center of 
gravity center of buoyancy separation is directly proportional to restoring mo- 
ment. Hydrodynamic coefficient (M Ss ) directly affects the pitch moment gener- 
ated by the dive planes. Speed was changed from 500 rpm to 1000 rpm. 



Table 5. TEST CASES FOR THE NONLINEAR MODEL 



TEST NO 


Ua 


Z* 


U ds 


RPM 


Test 1 


2\M q 


4 




500 


Test 2 


2*.\/, ? 


Z, 2 


2*M ds 


500 


Test 3 


u. 




2 *M* 


500 


Test 4 


\1 0 2 


Z 0 

- 


M d 2 


500 


Test 5 




z. 


2 


500 


Test 6 




Z e 4 




500 


Test 7 


M, 


Z* 




1000 



The controller used was based on the nominal linear model, while some vehicle 
coefficients differed by 200° « and nonlinear terms were added. The FORTRAN 
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program in Appendix B was used for this purpose. Time plots of depth, pitch 
rate, sliding surface, and pitch angle response along with the depth commanded 
values are shown from Figure 28 on page 62 to Figure 33 on page 67. 

The sliding mode controller is shown to provide stable response and performs 
consistently at different parameter values and speed. In every case, the system 
remains inside the boundary layer (a = 0). It is felt that the sliding mode method 
can produce extremely robust controllers that perform predictably despite the use 
of simplified or unmodeled dynamics. The center of gravity of the AUV is the 
most sensitive parameter in the vehicle dynamic response. The depth response 
of the AUV did not overshoot, although the center of gravity was close to the 
center of buoyancy. In general, the rpm affected the dynamic response of the 
nonlinear vehicle. In this case, although the rpm was doubled in magnitude, the 
overshoot was not present and the response time reduced to 35 seconds. When 
variation of parameters and modeling error of the AUV were increased, the 
robustness of the sliding control was improved by increasing the nonlinear feed- 
back gain Op). Of course, if the uncertain values exceed the limited boundary, 
the sliding mode control will not handle these variations effectively. The chosen 
nonlinear feedback gain (if- = 2.4) for the selected AUV was enough to handle the 
nonlinear terms and the modified hydodynamic coefficients. The modeling er- 
rors, variations of parameters, and nonlinear terms, which were difficult problems 
of robust using other control technique, were easily dealt with using the sliding 
mode control as shown from the results of simulations. 
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P I TCH fiNGLE ( RRD/SEC ) DEPTH ( FT ) 

- 1.1 - 0.3 - 0.5 - 0.2 0.1 - 5.0 20.0 45.0 70.0 95.0 120.0 







in 





I igure 2«S. I lie dynamic response of test 1 AUV (depth commnnd— 100 It) 
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Figure 29. The dynamic response of test 2 AUV (depth command = !()() f t ) 
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figure 30. The dynamic response of test 3 AUV (depth command = 100 ft) 
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Figure 31. The dynamic response of test 4 AUV (depth command = 1 00 It) 
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Figure 32. The dynamic response of test 5 AUY (deptli command = 100 ft) 
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Figure 33. The dynamic response of test 6 Al Y (depth command^ 1U0 It) 
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Figure 34. Tlie dynamic response of test 7 AUV (rpm= 1000) 
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IV. DESIGN AND EVALUATION OF A SLIDING MODE 

COMPENSATOR 



A. GENERAL 

An autonomous underwater vehicle (AUV) must operate under its own power 
and be capable of navigation and guidance with sufficient accuracy to be easily 
recoverable. A navigation system for an underwater vehicle is subject to vehicle 
size or cost restrictions and this limits the ability to install the highly accurate 
sensors needed to produce reliable pitch rate data. The vehicle depth can be 
measured directly by a pressure cell sensor aboard the vehicle. Using the depth 
of the vehicle as the only external input, state observers can provide all remaining 
controller data. It is desirable to investigate the performance of a sliding mode 
compensator designed for a linear model and applicable to a nonlinear model 
with unmodeled vehicle dynamics. It is the purpose of this chapter to design 
such a sliding mode compensator and assess its robustness. This work involves 
numerical simulation of the performance of a sliding mode compensator scheme 
using a linear and a lull 12 state nonlinear model of the equation of motion for 
the AUV. 

B. DESIGN OF SLIDING MODE COMPENSATOR 
I. Linear Model 

A state observer is designed which uses measured depth only in order to 
estimate the vehicle pitch rate and pitch angle. The block diagram of the basic 
sliding mode compensator is illustrated in Figure 35 on page 71. The state ob- 
server design is based on the equation [Ref. 1 1]. 

x = Ax Bu-\- Aq(t — Cx) (4.1) 

where x is the state variable estimated by the observer, and K 0 
is the observer feedback which is required for the observer to be able to follow 
and duplicate control system operation. 
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A. B, and C are the original open-loop control matrices of 



equation (3.1 1). 

u is the sliding mode control law based on the observer. 

Note that in the sliding mode compensator, disturbance, nonlinear terms, and 
variation of parameters are ignored. Equation (4.1) is schematically depicted in 
Appendix A. Collecting terms and rearranging 



The values for observer feedback gain (K 0 ) are calculated using the Matrix-x 
program "OBSERVER" in Appendix A. This provids three values of gain for the 
one state (depth) that is used by the observer in order to estimate the remaining 
two (pitch rate and pitch angle). The poles chosen for the observer are -4.5. -4.75, 
and -4.95. These values are selected because they create a faster response in the 
observer than in the controller itself. This condition is necessary to ensure that 
the observer will not slow down the overall simulation speed of the controller. 
The sliding surface of the sliding mode compensator has the same form as the 
sliding surface of a controller cr(.v) = S T x. 

The state observer provides necessary information to the sliding mode controller. 
The control input of the sliding mode compensator can be described in the fol- 
lowing process. From the sliding condition we ha\e 



A 



— ( A — K.QC)x-\-BuA-Koy 



(4.2) 



which is then rearranged into the familiar state space form: 




A A A 

y = Cx+DU 



(4.3) 



A 



where A is the observer A matrix (A — K 0 C) 



B is the observer B matrix 



C is the observ er C matrix 



7U 
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(j(-f) = -,j 2 sign(a) 



(4.4) 



Substituting equation (4.3) to equation (4.4) and rearranging 

S T x = —t] 2 sign(o) 



S T (Ax+Bu) = —ifsign(o) (4.5) 

Finally, the sliding mode control law can be expressed in terms of the estimated 
state variables in the following form: 

u = -{S r Br l S T Ax-{S T B)~ l >fsign(c) (4.6) 



or 



u = u+u 

where u = -{S T B)~'S T Ax 

U = —(S T B)~ ] >] 2 sign(a) 

The Figure 36 shows the dynamic response of the vehicle with the sliding mode 
compensator. The nonlinear feedback gain (i] : ) in the compensator is the same 
as the gain in the controller without the observer. It can be seen that since the 
observer poles are fast, the performance of the current compensator resemble that 
of the controller alone of Chapter 3. A sliding mode compensator is easier to 
design and implement than conventional compensators. 



o 








Figure 36. T lie dynamic response of the vehicle with sliding mode compensator. 
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2. Nonlinear Model 

The designed nonlinear control feedback gain (?/ 2 ), sliding surface (cr(jc)), 
and observer feedback gain (A' 0 ), which are based on the nominal linear model, 
are used to design a sliding mode compensator in order to handle a full nonlinear 
model. Since a nonlinear model can never be as easily predictable as a linear 
model, there exists certain degree of uncertainty in the nonlinear terms. A similar 
process as in Chapter 3 provides a design of a nonlinear sliding mode 
compensator. The AUV nonlinear dynamics is given by 



where A/bv) , B(x) are the model errors of nonlinear terms and 

uncertainty. 

Equation (4.7) can be rearranged by using the same proceedure in Chapter 3. 



The procedure for obtaining „v(/) of x(t) is to compute the estimate to be the out- 
put of the dynamic system. 



x = A x+Af[x) +B(x)u 



(4.7) 



Ax+AJ{x)+[lBu 



(4.8) 



A 

X 



= A a*+ A/(a*)+/? Bu-\- Aq(v — Cx ) 



(4.9) 



We can rewrite (4.9) in the following state space form 



x = Ax+AJ\x)+p Bu 




(4.10) 



where Fix) > | S T Afix) 
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The closed-loop dynamics of the state observer on the sliding surface with un- 
certainty is given by 



• / T a 

<7(XJ = S X 



= S T [Ax+AJ{x)+pBu ] ( 4 . 1 1 ) 

Substituting equation (4.6) to the above (4.1 1) for u, then we obtain the following 
equation 



cr = (1- p~ l )S T Ax+S T Af\x ) 

-p~ l ifsignc(x) (4.12) 

If g < —ifsigno(x) is enforced, stability will result. We can establish conditions 
on the use of i f in equation (4.6) that will guarantee that sliding condition, given 
the bounds of uncertainty. It follows that 

f a P(ul+S T Atf)+(\-r')S T Ax) (4.13) 

which can be achieved by 

if a ft™ I <)o+n.v) I + I (ft™- 1) 1 1 s t ax | (4.14) 

These uncertainties in equation (4.10) are eliminated by using equation (4.6) but 
with large enough gain (if) in order to guarantee stability. The boundary layer 
used in numerical simulation was selected in order to have the interpretation of 
smoothing out the discontinuity in the nominal control law at the switching sur- 
face. The nominal control law considered in this section. 



u = -lS T BY l S T Ax-lS T BY l i 1 2 satsign( 



(4.15) 



where if > /i max | /^-fTbv) j + | (/i max — 1) | j S T Ax 
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guarantees asymptotic stability for the nonlinear model which have state observer 
as shown in Figure 37. The nonlinear feedback gain (?/ 2 ) was chosen according 
to the assumed bounding nonlinear terms. The selected gain (?/ 2 ) in the previous 
chapter is large enough to handle any unknown bounded uncertainty. The Fig- 
ure 37 depicts the expected robustness of the dynamic response using sliding 
mode compensator. This method produces an extremely robust sliding mode 
compensator that performs predictably despite the use of the sliding surface, gain 
(?p), control law, and observer feedback gain (K 0 ) which are based on the nominal 
nlinear model. The FORTRAN program "OBSERVER" in Appendix B has been 
written to implement the sliding mode compensator. The results of Figure 37 on 
page 77 were obtained by using the standard hydrodynamic coefficients of the 
nonlinear model at 500 rpm. 
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Figure 37. The dynamic response of the sliding mode compensator for t lie standard 



C. ROBUSTNESS TESTS 
1. Linear Model 

In the previous numerical analysis we saw that sliding mode compensator could 
deal with unknown nonlinear terms by using nonlinear feedback control law u. 
A modified system matrix A. and control matrix B is used to estimate robustness 
performance of the sliding mode compensator in this section. The effects of 
modeling error in a linear model can be compensated by considering the size of 
modeling error as in the classical state space methods [Ref. 1 1]. The variations 
in control matrix B can be handled using a similar method. This technique is easy 
to analyze for a single input system. The effects of modeling error can be consid- 
ered for any system using sliding mode compensator. Consider the effect of a 
variation in the system matrix A and control matrix B. The system dynamics are 
given by 



x - (.4+(5A)a-+(£+<5£)k (4.16) 

where <5.4 is the unknown bounded changing system matrix 
SB is the unknown bounded changing control matrix 

The unknown changed system matrix (SA) and control (SB) matrix can be ac- 
commodated by using a large nonlinear (if) and boundary layer (<f>). The large 
gain will guarantee stability and the boundary layer obtained in the previous 
chapter will eliminate the chattering which is intrinsically linked to the use of a 
switching surface. The FORTRAN program "OBSERVER" in Appendx A is 
used to estimate unmeasurable state and perform sliding mode compensator to 
the modified system. The same boundary layer ((f)). nonlinear feedback gain (if), 
control law (4.6). and observer gain(/v 0 ) as in the nominal system are applied to 
the modified system to estimate the performance of a sliding mode compensator 
designed on a nominal linear model. The system matrix A and control matrix B 
are modified as in the following Table 6. 
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Table 6. ROBUSTNESS TESTS CASE OF THE SLIDING MODE 
COMPENSATOR 



Test No 


System matrix A 


Control matrix B 


Speed 


Test 1 


2* A 


B 


6 ft. sec 


Test 2 


A 2 


B 


6 ft sec 


Test 3 


A 


2*B 


6 ft see 


Test 4 


A 


B 2 


6 ft sec 


Test 5 


2*A 


2*B 


6 ft/ sec 


Test 6 


A 2 


B 2 


6 ft sec 



The dynamic response for each lest is shown in Figure 38 on page 80 through 
Figure 43 on page 85. Although the system matrix A and control matrix B are 
modified by 200%. there is only 10 ~ 11% overshoot to the test 5. The expected 
robustness of the dynamic response is presented by using sliding mode 
compensator based on nominal linear model. Sliding mode compensator per- 
formance is verified for a linear vehicle with uncertain A and B matrix without 
considering additional design. 

The sliding mode compensator developed in this section was stable and insensi- 
tive to change in hydrodynamic coefficients of the AUV. It therefore appears that 
variable structure systems will provide the most robust design for a sliding mode 
compensator that needs to maintain an accurate prediction of vehicle response 
under varying conditions. 
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Figure 38. The dynamic response of test I AUV (deptli command = 100 ft) 
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Figure 39. The dynamic response of test 2 AIJV (depth command= 100 ft) 
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figure 40. The dynamic response of test 3 AIJV (depth command = 100 ft) 
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Figure 41. The dynamic response of test 4 AUV (depth command = 100 ft) 



S3 



o 








ro lo 





figure 42. Hie dynamic response of test 5 AUV (depth command = 10(1 ft) 
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Figure 43. T he djnamic response of test 6 AUV (depth command = 101) ft) 



2. Nonlinear Model 



The siding mode compensator based on the nominal linear model is here applied 
to the highly nonlinear vehicle with modified hydrodynamic coefficients in order 
to estimate its robustness. 

The hydrodynamic coefficients, which affect in the depth change maneuvering, 
are modified as Table 5 in the previous chapter. Although an unmodeled dis- 
turbance could be made up for using model based compensator, parameter mis- 
match was determined to be sensitive to the used method [Ref. 12]. This would 
reduce the robustness of the controller when confronted with the varying, un- 
controlled condition found in the ocean enviroment. In this section, sliding mode 
compensator is used to investigate robustness for control of depth change ma- 
neuvering in the face of unmodeled nonlinear terms and parameter uncertainty. 
The nonlinear system dynamics for this purpose were described by 



Since SA, SB. J\x), and g(.v) are unknown values in the present system equation 
described above, they are assumed to be zero and tf is increased through numer- 
ical simulation in order to guarantee sliding mode control [Ref. 2]. The nominal 
control law (4.6) is used to simulate the response of all values (q, u. z, and s) both 
actual and estimated. The FORTRAN program "NSMC" was written in order 
to simulate numerical experiments provided in Appendix B. Note that in this 
section, a soft saturation function might be used to handle the highly nonlinear 
model within the boundary layer. The soft saturation function is 



This saturation function produces soft inputs at approximately zero c(x) values. 
The dynamic response of the highly nonlinear AUV is shown in Figure 44 on 
page 87 through Figure 49 on page 92. 



v = .4.Y+d/4vT/(A')T£ 5-h(5.64"g(.Y)]it 



(4.17) 







(4.18) 
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figure 4-1. T lie thnnmic response of nonlinenr test 1 AUV 
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figure 45. 1 lie dynamic response of nonlinear test 2 AUV 
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Figure 46. 1 lie (hnamie response of nonlinear test 3 AUV 
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Figure 47. The dynamic response of nonlinear test 4 AUY 
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Figure 4S. 1 lie dynamic response of nonlinear test 5 AUV 
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Figure 49. The dynamic response of nonlinear test 6 AUV 



V. CONCLUSIONS AND RECOMMENDATIONS 



A. SUMMARY 

This thesis presented an application of variable structure systems for the ro- 
bust control of linear and nonlinear systems in the presence of disturbances and 
parameter variations. The designed sliding mode compensator based on this 
methodology was successfully employed to provide a means for an unmanned 
underwater vehicle to control its depth under high nonlinearities. In this section, 
we summarize the sliding mode compensator design of the specified AUV: 

1. The fully nonlinear equations of motion are linearized and simplified (as il- 
lustrated in section 3.C) in order to produce linear equations in the state 
space form. 

2. The sliding surface, which provides desired dynamics of a closed-loop sys- 
tem, is designed by the standard pole placement method using the state 
space form. 

3. The sliding mode control law, which consists of linear feedback and nonlin- 
ear switching feedback, can be obtained by satisfying the sliding condition. 

4. Chattering due to piecewise discontinuous feedback is eliminated by replac- 
ing the switched control law at the sliding surface by continuous variations 
across a thin boundary layer neighboring the switching surface. 

5. The nonlinear feedback gain 0/ 2 ) and boundary layer (</>) are optimized by 
a series of numerical experiments. 

6. The state observer is designed using classical methods. 

7. The sliding mode compensator is designed by combining the sliding mode 
control and state observer. 

B. CONCLUSIONS 

The objective of this work to design and analyze a sliding mode dice plane 
compensator for an autonomous underwater vehicle has been achieved. Realistic 
limitations due to pitch and pitch rate sensors non-availability were taken into 
account. The vehicle that was considered, the SDV is a typical AUV and its 
shape and characteristics greatly resemble the NPS vehicle. 

The conclusions of this work are summarized below: 
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1. A procedure for computing the sliding plane was established. The procedure 
is very general and can be applied to a large class of linearized single input 
systems. Once the sliding plane coefficients have been determined, the 
switching feedback control law follows easily. 

2. Chattering problems, characteristic of variable structure system, were re- 
duced or eliminated by introducing a "boundary layer" in the switching logic, 
without violating the sliding condition. 

3. Robustness of the design was demonstrated by a wide variation of parame- 
ters of the linear model. Similar robustness characteristics were established 
when a full order observer was incorporated in the design in order to provide 
estimates for pitch angle and pitch rate based on depth measurements only. 

4. The control design that was based on a linear model was tested against the 
full nonlinear equations of motion. Numerical simulations demonstrated the 
ability of the design to handle unmodelled dynamics and variation of the 
hydrodynamic coefficients and geometric parameters of the vehicle. 

5. A visual simulation using an IRIS graphics workstation was used [Ref. 13] 
in order to view the dynamic behavior of the AUV in real time under 
closed-loop sliding mode control. The vehicle response, Figure 50 on page 
95, was seen to follow the predictions that were based on the linear model. 

6. Finally, an experimental verification was attempted on the NPS protype ve- 
hicle with coefficients the same as the ones used in [Ref. 12 ]. A discrete- 
time sliding mode controller was designed based on 25 Hz sample rate with 
controller poles at [0.9,0.91, 1], observer poles at [0.78,0.79,0.80], control 
law 



u = -0.9<7 — 1 .7890—2 1 .291 sign{o) 

o = —0.3386(7 — 1. 6SS80+(z-z cow ) (5.1) 

where = 0.2 and <f> = 1.0 

The experimental results are shown in Figure 51 on page 96, where the 
commanded depth was 5 volts (1 ft corresponds to 3.1 volts). 

The variable structure system was proven to be an attractive control system de- 
sign method for autonomous underwater vehicles. The designed sliding mode 
compensator based on this methodology dealt with the dynamic problems of the 
underwater vehicle with sufficient accuracy. 
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Figure 50. Graphic simulation display 



C. RE CO MM £N DA HONS 



Some suggestions for future research are as follows: 



3. Design and analyze a nonlinear sliding mode compensator. 

2. Evaluate the robustness characteristics of a digital (discrete time) sliding 
mode autopilot. 

3. Extend the developed methodology in the problem of path following 
autopilots for the multiple input case under constant or random disturbance. 
Introduction of integral control action should help in this case to assure 
precise path tracking. 
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1 lie experimental results of the sluliu^ moil e compensator 
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APPENDIX A. SIMULATION PROGRAM AND BLOCK DIAGRAM FOR 

LINEAR EQUATION 

A. MATRIX-X(SMC) OF THE LINEAR EQUATION 



**** MATRIX PROGRAM FOR SLIDING CONTROLLER *** 



UX=6 ; 


// 


FORWARD 


SPEED 


CP1=0 .25; 


// 


CLOSED 


LOOP POLE 


CP2 = 0 . 27 ; 


// 


CLOSED 


LOOP POLE 


Sl=l . ; 


// 


SLIDING 


SURFACE COEFFICIENT 


S2 = ( CP1 + CP2 ) ; 


// 


SLIDING 


SURFACE COEFFICIENT 


S3=(CPl*CP2)/(-UX) 


// 


SLIDING 


SURFACE COEFFICIENT 



T= [0:0. 1:150.]'; 

I ETA =6.0; 

GA3=-0 . 035; 

GA4=-UX ; 

GA5=- 0 . 7 ; 

GA6=- 0 . 03 ; 

GA7=S2 ; 

GA8 = S3 ; 

GA9 = I ETA ; 

GA10 = 1 ; 

GAl= ( GA6-S3 *UX )/( -GA3 ) ; 

GA2= ( GA5+S2 ) /( -GA3 ) ; 

INPUT ( : , 1 )=100*ONES(T) ; 

INPUT ( : , 2 ) = 0 . *ONES ( T ) ; 

Y=S IM ( T , INPUT) ; 

PLOT] T, Y( :, 1 ) f ' UPPER LEFT XLABEL/TIME ( SEC )/ YLABEL/ . . . 

DEPTH RESPOSE/TITLE/ DEPTH COMMAND= 100 , STANDARD AUV/ ' ) 

PLOT(T,Y( : , 2 ) , 'UPPER RIGHT XLABEL/TIME ( SEC )/ YLABEL/ ... 

PITCH ANGLE/TITLE/ CLOSED LOOP POLE=0 . 0 , - 0 . 3 5 , - 0 . 4 0/ ' ) 
PLOT( T, Y( : , 4 ) , ' LOWER LEFT XLABEL/TIME ( SEC )/ YLABEL/ ... 

DIVE PLANE ANGLE] radins ) /TITLE/ SPEED=6/') 

PLOT] T, Y( :, 5 ),' LOWER RIGHT XLABEL/TIME] SEC )/YLABEL/ ... 
SLIDING SURFACE/TITLE/ DIS=0.2, B, NLI NEAR , I ETA=6 . 0 , PI= 0 . 4/ ' ) 



oo onnnnno 



B. FORTRAN(SMC) OF THE LINEAR EQUATION 



LINEAR AUV MODEL 

SLIDING MODE CONTROL PROGRAM * 

DEPTH CONTROL IN DIVE PLANE * 

BY USING RUNGE-KUTTA FIFTH ORDER * 



REAL* 8 TIME, QDOT , ZDOT, TDOT, THETA, ZPOS ,Q,DS 
REAL*8 S, DE, UHAT, UBAR,PHI ,SAT,EITA,COMZ 
********* initial CONDITIONS ********** 



C 



C 



710 

C 



C 



720 

100 



TIME=0. 0D0 



Q=0. 0D0 
THETA=0. ODO 
ZP0S=0. ODO 






SLIDING MODE CONTROL PARAMETER *********** 



EITA=4. ODO 



DELT=0. 02D0 
COMZ=100. ODO 
PHI=0. 4D0 

******* SYSTEM PROGRAM ******** 

WRITE (8,710) 

DO 100 1=0,6000 

FORMAT ( 3X , ' TIME',5X,’ DEPTH ' ,6X,’ DIVE ' 

*’ , 5X, ' SLIDE’) 



5X,’ 



QDOT=-0. 7D0*Q-0. 03D0--THETA-0. 035D0*DS 
TD0T=Q 

ZDOT=- 12. 0D0*THETA 



T I ME=T I ME+DE LT 
Q=Q+DELT*QDOT 
THETA=THETA+DE LT*TDOT 
ZPOS=ZPOS+DELT*ZDOT 

S=Q+0. 52D0*THETA-0. 01 12D0*( ZPOS-COMZ) 

IF (ABS(S) .LT. PHI) SAT=(S/PHI) 

IF (S .LT. -PHI) SAT= - 1. ODO 
IF (S . GT. PHI) SAT=1. ODO 
UHAT=-5. 1429D0*Q+1. 0714D0*THETA 
UBAR=EITA*SAT 
DE=UHAT+UBAR 

IF (DE . GE. 0.4D0) DS=0. 4D0 
IF (DE . LE. -0. 4D0) DS=-0. 4D0 

IF ((DE .LT. 0.4D0) .AND. (DE . GT. -0.4D0)) DS=DE 

WRITE (8,720) TIME, ZPOS, DS, THETA, S 

FORMAT (2X.E11. 3 ,4X ,E1 1. 3 , 4X, El 1. 3,4X,E11. 3,4X,E11. 3) 

CONTINUE 

STOP 

END 



PITCH 



9S 



C. MATRIX-X(OBSERVER) OF THE LINEAR EQUATION 



*** SIMULATION PROGRAM FOR SLIDING MODE COMPENSATOR *** 



UX = 6 ; 

CP1=0 . 3 
CP2=0 . 40 ; 

Sl=l . ; 

S2 = ( CP1+CP2 ) ; 
S3=( CPI *CP2 )/( 
El TA = 6.0; 

A= [-0.7, -0.03, 
B= [ -0 . 0 35 ; 0 ; 0 ] 
C= [0,0,1); 

N= [ C ' , A ' * C ' , A ' 
W= [ 1 , 0 . 7 , 0 . 0 3 ; 
LAI =5 . 5 ; 

LA2 = 5 .75; 

LA3=5 . 9 ; 

ABAR= ( 0 . 7 ; 0 . 03 
AHAT= [ LA1+LA2+ 
KC=INV ( ( N*W ) ' ) 
GAl=-l . 4 ; 
GA2=-0 . 06 ; 
GA3=-2*UX; 

GA4 = KC ( 3,1) 
GA5=KC (2,1 ) 
GA6=KC ( 1 , 1 ) 
GA7=-0 . 7 ; 
GA8=-0 . 03 ; 
GA9=-6 ; 

GAl 0=- 0 . 035 
GAl 6=-0 . 035 



- 0 . 6 ) ; 



// FORWARD SPEED 
// CLOSED LOOP POLE 
// CLOSED LOOP POLE 
// SLIDING SURFACE COEFFICIENT 
// SLIDING SURFACE COEFFICIENT 
// SLIDING SURFACE COEFFICIENT 



0 ; 1 , 0 , 0 ; 0 , -6 , 0 ] ; 



*A' *C' ] ; 

0 , 1 , 0 . 7 ; 0 , 0 , 1 ] ; 

// DESIRE POLE OF OBSERVER 
// DESIRE POLE OF OBSERVER 
// DESIRE POLE OF OBSERVER 

; 0 ] ; 

LA3 ; LAl * LA2 + LA2 * LA3 + LA1 * LA3 ; LAl*LA2*LA3 ] ; 
* ( AHAT-ABAR) ; 



GAl 1=0 .75; 

GAl 3=-0 . 0233; 

GAl 4 = ( GA2-S 3 *UX ) /( -GAl 0 ) ; 

GAl 2= ( GA7+S2 ) / ( -GAl 0 ) ; 

GAl 5=EITA; 

T= [0:0. 1:150.]'; 

INPUT ( : , 1 ) =1 0 0 *ONES ( T ) ; 

INPUT ( : , 2 ) =0 . 0*ONES(T) ; 

Y=SIM ( T, INPUT) ; 

PLOT ( T , ( Y ( : , 1 ) Y ( : , 5 ) ] , ' LI NE STYLE 1 2 /UPPER LEFT XLABEL/ 
TIME {sec) / YLABEL/EST AND ACT DEPTH (ft)/' ) 

PLOT ( T , [ Y ( : , 2 ) Y ( : , 4 ) ] , ' LI NE STYLE 1 2 /UPPER RIGHT XLABEL/ 
TIME(sec)/ YLABEL/EST AND ACT ANGLE ( r ad ) /TI TLE/ B, 2*A/') 
PLOT( T, Y( : , 3 ) , ' LOWER LEFT XLABEL/TIME ( SEC ) / YLABEL/ 

DIVE PLANE ANGLE/TITLE/ S . F . C=1 , 0.75, -0.0233, SPEED=6/') 
PLOT( T, Y( : , 6 ) , ' LOWER RIGHT XLABEL/TIME ( SEC )/YLABEL/ 

SLIDING SURFACE/ TITLE/ SAT, DIS=0.2, IETA=6 , PI=0 . 4/' ) 
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on ooooooo 



D. 



EORTRAN(OBSERVER) OF THE LINEAR EQUATION 



Vc Vc Vc Vc * Vc Vc Vc V r Vc Vc 



VrVc V? /V VcVr Vc Vc Vc Vc Vc Vc Vc Vc Vc Vc Vc Vc Vc Vc VcVc 



* LINEAR AUV MODEL 

* SLIDING MODE CONTROL PROGRAM 

* DEPTH CONTROL IN DIVE PLANE 

* BY USING EULER-METHOD 



Vc 'V Vc ‘V Vc Vc Vc * V V? 1 



/V V? ‘V V? *5 V Vc Vc Vc Vc Vc Vc V* ‘V Vr >V Vc V c Vc ' 



Vr Vr Vc Vc * Vc Vc Vr Vc * Vc 



REAL*8 TIME,QDOT, ZDOT, TDOT , THETA , ZPOS , Q , DS 
REAL*8 S, DE, UHAT, UBAR,PHI ,SAT,EITA,COMZ 
REAL*8 THADOT , QHADOT , ZHADOT , QHAT , THAT , ZHAT 

********* initial conditions ********** 



TIME=0. 0D0 
Q=0. 0D0 
THETA=0. 0D0 
ZPOS=0. 0D0 
QHAD0T=0. 0D0 
THAD0T=0. ODO 
ZHAD0T=0. ODO 
QHAT=0. ODO 
THAT=0. ODO 



ZHAT=0. ODO 

C ********* SLIDING MODE CONTROL PARAMETER *********** 
EITA=4. ODO 
DELT=0. 02D0 
C0MZ=100. ODO 
PHI=0. 4D0 

C ******* SYSTEM PROGRAM ******** 



WRITE (8,710) 

DO 100 1=0,6000 

710 FORMAT ( 3X , 1 TIME’,5X,' DEPTH ',6X,' DIVE ’,5X,' 

*' ,5X, ' SLIDE') 

C 

QD0T=-0. 35D0*Q-0. 015D0*THETA-0. 0175D0*DS 
TDOT=0. 5D0*Q 
ZD0T=-6. ODO*THETA 
C 

TIME=TIME+DELT 
Q=Q+DELT*QDOT 
THETA=THETA+DE LT*TDOT 
ZPOS=ZPOS+DELT*ZDOT 

C ******** SUBROUTINE OBSERVER ********* 

CALL OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , DELT) 

C ******* SLIDING MODE INPUT ********* 



S=QHAT+0. 52D0*THAT-0. 0112D0*(ZHAT-C0MZ) 
IF (ABS(S) .LT. PHI) SAT=(S/PHI) 

IF (S .LT. -PHI) SAT=- 1. ODO 
IF (S .GT. PHI) SAT=1. ODO 
UHAT=-5. 1429D0*QHAT+1. 0714D0*THAT 



PITCH 
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UBAR=EITA*SAT 

DE=UHAT+UBAR 

IF (DE . GE. 0. 4D0) DS=0. 4D0 
IF (DE . LE. -0.4D0) DS=-0. 4D0 

IF ((DE . LT. 0.4D0) .AND. (DE . GT. -0. 4D0)) DS=DE 
WRITE (8,720) TIME, ZPOS, DS, THETA, S 
720 FORMAT (2X,E11. 3,4X,E11. 3,4X,E11. 3,4X,E11. 3,4X,E11. 3) 

100 CONTINUE 
STOP 
END 

C ********* SUBROUTINE OBSER ********** 

c 

SUBROUTINE OBSER( QHADOT ,THADOT , ZHADOT ,QHAT , THAT , ZHAT , ZPOS , DELT) 
QHAD0T=-0. 7D0*QHAT-0. 03D0*THAT-20. 9293D0*( ZPOS-ZHAT) 
THADOT=QHAT- 14. 409 2D0*( ZPOS - ZHAT) 

ZHADOT=-6. 0D0*THAT+16. 45D0*( ZPOS-ZHAT) 

QHAT=QHAT+DELT*QHADOT 

THAT =THAT +DE LT*THADOT 

ZHAT=ZHAT+DELT*ZHADOT 

RETURN 

END 
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E. FORTRAN GRAPH 

DIMENSION TIME( 6000) ,DEPTH( 6000) , DIVE( 6000) , PITCH( 6000) , 
*SLIDE( 6000) 

REAL TIME , DEPTH , DIVE , PITCH , SLIDE 
DO 1 1=1,6000 

1 READ (8,*) TIME( I ) ,DEPTH( I) ,DIVE( I) , PITCH( I) , SLIDE( I ) 
CALL COMPRS 

C ***** CREATES A DUMMY PLOT FOR POST PROCESSING****** 

CALL NOBRDR 
CALL AREA2D( 1. ,1. ) 

CALL GRAF( 0. ,1. ,20. ,0. ,1. ,10) 

CALL ENDPL(O) 



C ***** plot COMMAND DEPTH ***** 

CALL PAGE( 3. ,4. ) 

CALL NOBRDR 

CALL AREA2D(2. 3,3. 0) 

CALL XNAME( ' TIME( SEC) $ 1 , 100) 

CALL YNAME( 'COMMAND DEPTH(FT)$ ' , 100) 

CALL THKFRM(0. 03) 

CALL GRAFCO. ,30. 0,120. , -5. 0 ,25. 0 , 120. 0) 

CALL THKCRV(0. 02) 

CALL CURVE(TIME, DEPTH, 6000,0) 

CALL ENDPL(O) 

C ***** PLOT DIVE PLANE ANGLE ***** 

CALL PAGE( 3. ,4. ) 

CALL NOBRDR 

CALL AREA2D( 2. 3,3. 0) 

CALL XNAME( ' TIME( SEC) $ ' , 100) 

CALL YNAME( ' DIVE PLANE ANGLE( RAD) $ ' , 100) 
CALL THKFRM(0. 03) 

CALL GRAF( 0. ,30.0,120. ,-.4,. 2,. 4) 

CALL THKCRV( 0. 02) 

CALL CURVE(TIME, DIVE, 6000,0) 

CALL ENDPL(O) 

C ***** PLOT PITCH ANGLE ****** 

CALL PAGE( 3. ,4. ) 

CALL NOBRDR 

CALL AREA2D( 2. 3,3. 0) 

CALL XNAME( ' TIME (SEC) $' ,100) 

CALL YNAME('PITCH ANGLE( RAD/SEC) 100) 

CALL THKFRM( 0. 03) 

CALL GRAF( 0. ,30. 0,120. , -1. 0 ,0. 3 ,0. 2) 

CALL THKCRV( 0. 02) 

CALL CURVE(TIME, PITCH, 6000,0) 

CALL ENDPL(O) 

C ***** plot SLIDING SURFACE ***** 

CALL PAGE( 3. ,4. ) 

CALL NOBRDR 

CALL AREA2D( 2. 3,3. 0) 

CALL XNAMEC 'TIME(SEC)$' ,100) 

CALL YNAME( 'SLIDING SURFACE( RAD/SEC) $ ' ,100) 



CALL THKFRM( 0. 03) 

CALL GRAF ( 0 . ,30. 0,120. , -0. 5 , 0. 5 , 1. 5 ) 
CALL THKCRV( 0. 02) 

CALL CURVE ( TIME, SLIDE, 6000,0) 

CALL ENDPL(O) 

CALL DONE PL 

STOP 

END 
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F. BLOCK DIAGRAM OF THE MATRIX-X SIMULATION 




f 

il 



tJ 

5 



l 







Figure 52. The block diagram of t lie SMC simulation 
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Figure 53. T lie block diagram of the SMO simulation 
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APPENDIX B. SIMULATION PROGRAM FOR NONLINEAR AUV 



•sWr 






* 



* NONLINEAR AUV MODEL / STERN PLANE AND BOW PLANE SEPARATED * 

Jm 



* VARIABLE DECLARATION 11 

* CONSTANTS 70 

* INITIAL CONDITIONS 128 

* MASS MATRIX 206 

* INVERT MATRIX 245 



** INPUTS ** 248 * 

PROPULSION MODEL 271 * 

** OUTPUTS ** 423 * 

INTEGRATION * 



****■ 









REAL AW( 82 , 82) 

REAL MASS ,LATYAW,NORPIT 

REAL MM( 6,6) ,G4(4) ,GK4(4) ,BR(4) ,HH(4) 

REAL B( 6 , 6) , BB( 6 ,6) 

REAL A( 12 , 12) , AA(12,12) 

REAL XPP , XQQ ,XRR ,XPR 
REAL XUDOT ,XWQ ,XVP ,XVR 
REAL XQDS ,XQDB ,XRDR ,XVV 
REAL XVW , XVDR ,XWDS ,XWDB 
REAL XDSDS , XDBDB , XDRDR ,XQDSN 
REAL XWDSN ,XDSDSN 

REAL TIME , S , E ITA , UBAR , UHAT , COMZ , BAR , S IM , DE , S AT 



LATERAL HYDRODYNAMIC COEFFICIENTS 



REAL YPDOT , YRDOT, YPQ ,YQR 
REAL YVDOT ,YP , YR ,YVQ 
REAL WP , YVR ,YV ,YVW 
REAL YDR ,CDY 

NORMAL HYDRODYNAMIC COEFFICIENTS 



REAL ZQDOT ,ZPP,ZPR ,ZRR 
REAL ZWDOT ,ZQ ,ZVP ,ZVR 
REAL ZW ,ZVV , ZDS ,ZDB 
REAL ZQN , ZWN ,ZDSN ,CDZ 
REAL ZHADOT, ZHAT 

ROLL HYDRODYNAMIC COEFFICIENTS 

REAL KPDOT , KRDOT ,KPQ ,KQR 
REAL KVDOT , KP , KR ,KVQ 
REAL KWP , KVR ,KV ,KVW 
REAL KPN , KDB 

PITCH HYDRODYNAMIC COEFFICIENTS 



REAL MQDOT ,MPP , MPR , MRR 
REAL MWDOT , MQ ,MVP ,MVR 
REAL MW , MW ,MDS ,MDB 

REAL MQN , MWN ,MDSN 
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REAL QHADOT , QHAT , THADOT , THAT 

YAW HYDRODYNAMIC COEFFICIENTS 

REAL NPDOT , NRDOT , NPQ ,NQR 
REAL NVDOT , NP ,NR ,NVQ 
REAL NWP , NWR ,NV ,NW 
REAL NDR 



MASS CHARACTERISTICS OF THE FLOODED VEHICLE 

REAL WEIGHT , BOY ,VOL ,XG 

REAL YG , ZG ,XB ,ZB 

REAL IX , IY ,IZ ,IXZ 

REAL IYZ , IXY , YB 

REAL L , RHO ,G ,NU 

REAL AO ,KPROP ,NPROP , X1TEST 

REAL DEGRUD ,DEGSTN 

COMMON /BLOCK1/ F(12), FP(6), XMMINV(6,6), UCF(4) 
INTEGER N , IA , IDGT , IER , LAST, J ,K,M , JJ ,KK , I 
REAL WKAREA(54), X(12) 

RUDDER COEFFICIENTS 

PARAMETER ( DSMAX= -0.175) 

LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 



-1. 5E-2 ,XRR = 4. E-3 ,XPR=7.5E-4, 



PARAMETER( XPP = 
Sc XUD0T=-7. bE-3 
& XQDS=2. 5E-2 
Sc XWW =1. 7E-1 
Sc XDSDS= -l.E-2 
Sc XWDSN=3. 5E-3 



7. E-3 ,XQQ = 

, XV Q = -2. E-l 
,XQDB=-2. 6E-3 
, XVDR=1. 7E-3 
,XDBDB= -8. E-3 
, XDSDSN= -1.6E-3 



,XVP = -3. E-3 
,XRDR= -1. E-3 
,XWDS=4. 6E-2 
, XDRDR= -l.E-2 
) 



,XVR = 2.E-2, 
,XVV =5. 3E-2, 

, XWDB= l.E-2, 
,XQDSN= 2. E-3, 



LATERAL HYDRODYNAMIC COEFFICIENTS 



PARAMETER ( YPD0T=1. 2E-4 ,YRD0T=1. 2E-3 
Sc YVD0T=-5. 5E-2 ,YP =3. E-3 , YR 

Sc YWP =2. 3E-1 , YWR =-l. 9E-2 ,YV 

& Y'DR =2. 7E-2 ,CDY =3. 5E-1) 



, YPQ = 4. E-3 ,YQR =-6. 5E-3, 
= 3. E-2 , YVQ =2. 4E-2 , 

= -1. E-l , YVW =6. 8E-2 , 



NORMAL HYDRODYNAMIC COEFFICIENTS 



PARAMETER( ZQD0T=-6. 8E-3 ,ZPP =1. 3E-4 ,ZPR=6.7E-3 , ZRR =-7. 4E-3 , 

Sc ZVD0T=-2. 4E-1 ,ZQ=-1.4E-1 ,ZVP=-4.8E-2 ,ZVR=4.5E-2, 

Sc ZW = -3. E-l ,ZVV =-6. 8E-2 ,ZDS =-7. 3E-2 ,ZDB =-2.6E-2, 

Sc ZQN =-2. 9E-3 ,ZWN =-5. IE-3 ,ZDSN= -l.E-2 ,CDZ = 1.0) 



ROLL HYDRODYNAMIC COEFFICIENTS 



PARAMETER( KPDOT= 
Sc KVD0T=1. 3E-4 
Sc KVP =-l. 3E-4 
Sc KPN =-5. 7E-4 , 



-1. E-3 ,KRD0T=-3. 4E-5 ,KPQ =-6. 
KP =-l. IE-2 ,KR =-8. 4E-4 
KVR =1. 4E-2 ,KV =3. IE-3 
KDB = 0.0) 



9E-5 ,KQR =1. 7E-2 , 
,KVQ=-5. IE-3, 

,KVW =-l. 9E-1 , 
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PITCH HYDRODYNAMIC COEFFICIENTS 



PARAMETER(MQDOT=-l. 7E-2 ,MPP =5.3E-5,MPR = 
& MVDOT=-6. 8E-3 , MQ =-6. 8E-2 ,MVP =1. 2E-3 
& MW = l.E-1 , MW =-2. 6E-2 ,MDS =-4. 1E- 
& MQN =-l. 6E-3 , MWN =-2. 9E-3 ,MDSN =-5. 2E 

YAW HYDRODYNAMIC COEFFICIENTS 



PARAMETER(NPDOT=-3. 4E-5 ,NRD0T=-3. 4E-3 ,NPQ =- 
& NVDOT=l. 2E-3 , NP =-8. 4E-4 ,NR =-1.6E-2 
& NWP =- 1. 7E-2 , NWR =7. 4E-3 ,NV =-7.4E-3 
& NDR =-l. 3E-2) 

MASS CHARACTERISTICS OF THE FLOODED VEHICLE 



& 

& 

& 

&. 

& 

& 



PARAMETER( WEIGHT =12000. , BOY =12000. ,VOL = 



YG = 0.0 
IX = 1500. 
IYZ = -10. 

L = 17.4 
AO = 2. 0 
DEGRUD= 0. 0 



, ZG = 0. 2 
, IY = 10000. 

, IXY = -10. 

, RHO = 1. 94 
,KPROP = 0. 

, DEGSTN= 0.0) 



,XB = 0. 

,IZ = 10000. 
,YB = 0. 0 , 

,G = 32. 2 
, NPROP = 0. 



INPUT INITIAL CONDITIONS HERE IF REQUIRED 



UO = 12. 0 
VO = 0. 0 
WO = 0.0 
PO = 0. 0 
QO = 0. 0 
RO = 0. 0 
PHIO = 0. 0 
THETAO = 0.0 
PSIO = 0. 0 
XP0S0=0. 0 
YP0S0=0. 0 
ZP0S0=0. 0 
DB= 0. 0 
DS = 0. 0 
DR = 0. 0 
RPM = 1000. 0 
LATYAW = 0. 0 
NORPIT = 0.0 
RE = UO*L/NU 



U = UO 
V = VO 
W = WO 
P = PO 
Q = QO 
R = RO 

XPOS = XPOSO 
YPOS = YPOSO 
ZPOS = ZPOSO 
PSI = PHIO 



5 . E - 3 , MRR =-2. 9E-3, 
,MVR =1. 7E-2 , 

2 ,MDB =6. 9E-3, 
-3) 



2. IE-2 ,NQR =2. 7E-3, 
,NVQ = -l.E-2, 
,NVW =-2. 7E-2 , 



200. ,XG = 0. , 

, ZB = 0.0 , 

,IXZ = -10. , 

,NU = 8.47E-4 , 
, X1TEST= 0. 1 , 



THETA = THETAO 
PHI = PHIO 
QHADOT=0. 0 
THAD0T=0. 0 
ZHAD0T=0. 0 
QHAT=0. 0 
THAT=0. 0 
ZHAT=0. 0 

DEFINE LENGTH FRACTIONS FOR GAUSS QUADRATURE TERMS 

G4( 1) = 0.069431844 
G4( 2) = 0. 330009478 
G4( 3) = 0. 669990521 
G4( 4) = 0. 930568155 

DEFINE WEIGHT FRACTIONS FOR GAUSS QUADRATURE TERMS 

GK4( 1 ) = 0. 1739274225687 
GK4( 2) = 0. 3260725774312 
GK4( 3) = 0. 3260725774312 
GK4( 4) = 0. 1739274225687 

DEFINE THE BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 

BR( 1) = 75. 7/12 
BR( 2) = 75. 7/12 
BR( 3) = 75. 7/12 
BR( 4) = 55. 08/12 

HH( 1 ) = 16. 38/12 
HH( 2) = 31. 85/12 
HH( 3) = 31.85/12 
HH( 4) = 23. 76/12 

MASS = WEIGHT/G 



N = 6 

DO 15 J = 1 ,N 
DO 10 K = 1 ,N 
XMM1NV( J ,K) =0.0 
MM( J , K) =0.0 
10 CONTINUE 

15 CONTINUE 



MM( 1,1) = MASS - ( ( RHO / 2 ) *( L**3 ) *XUDOT) 
MM( 1,5) = MASS*ZG 
MM( 1,6) = -MASS*YG 
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MM( 2,2) = MASS -((RH0/2)*(L**3)*YVD0T) 

MM( 2,4) = -MASS*ZG - ( ( RHO / 2 ) * ( L**4 ) * YPDOT ) 
MM( 2,6) = MASS*XG - ((RHO/2)*(L**4)*YRDOT) 

MM(3 ,3) = MASS - ((RHO/2)*(L**3)*ZWDOT) 

MM (3,4) = MASS*YG 

MM( 3,5) = -MASS*XG -((RH0/2)*(L**4)*ZQD0T) 

MM(4,2) = -MASS*ZG - ( (RH0/2)*(L**4)*KVD0T) 
MM (4, 3) = MASS*YG 

MM(4,4) = IX - ((RHO/2)*(L**5)*KPDOT) 
MM(4,5) = -IXY 

MM( 4,6) = -IXZ -((RHO/2)*(L**5)*KRDOT) 

MM( 5,1) = MASS*ZG 

MM( 5,3) = -MASS*XG -( (RHO/2)*(L**4)*MWDOT) 
MM( 5,4) = -IXY 

MM(5 ,5) = IY -((RHO/2)*(L**5)*MQDOT) 

MM(5 ,6) = -IYZ 



MM( 6,1) = -MASS*YG 

MM( 6,2) = MASS*XG -( (RHO/2)*( L**4)*NVD0T) 
MM( 6,4) = -IXZ - ((RHO/2)*(L**5)*NPDOT) 
MM( 6,5) = -IYZ 

MM( 6,6) = IZ - ((RHO/2)*(L**5)*NRDOT) 



LAST = N*N+3*N 
DO 20 M = 1 ,LAST 
WKAREA(M) =0.0 
20 CONTINUE 



IER = 0 
IA = 6 
IDGT = 4 

CALL LINV2F(MM,N,IA,XMMINV, IDGT, WKAREA, IER) 



j INPUTS Vwf Vv Vc Vf Vc Vc Vc VcVc 

RUDDER AND DIVE PLANE COMMANDS 



DELT=0. 1 
SIM= 800. 0 
TIME= 0. 0 
DS= 0.0 
DR= 0. 0 
DB= 0. 0 
RPM=1000. 0 
EITA=6. 0 
COMZ=100. 0 
BAR=0. 4 

c SIZE OF OUTPUT DATA ARRAY FOR PLOTTING 
c NUMOUT=6 

c NUMPNT=4 

c WRITE (8,711) NUMPNT , NUMOUT 
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P^P^P^P^P^P^P^P" 



WRITE (8,710) 

711 FORMAT (214) 

DO 100 1=1, SIM 
C 
C 
C 

C PROPULSION MODEL 

C 

C 

SIGNU =1.0 

IF (U. LT. 0. 0) SIGNU = -1. 0 
IF (ABS(U). LT. X1TEST) U = X1TEST 
SIGNN = 1. 0 

IF (RPM. LT. 0.0) SIGNN = -1.0 
ETA = 0. 012*RPM/U 
RE = U*L/NU 

CDO = .00385 + ( 1. 296E-17)*(RE - 1. 2E7)**2 

CT = 0. 008*L**2*ETA*ABS(ETA)/(A0) 

CT1 = 0. 008*L**2/(A0) 

EPS = -1. 0+SIGNN/SIGNU*(SQRT(CT+l. 0)-l. 0)/(SQRT(CTl+l. 0)-l. 0) 
XPROP = CDO*(ETA' v ABS(ETA) - 1.0) 



CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A 4 TERM GAUSS QUADUTURE 



LATYAW = 0. 0 
NORPIT = 0. 0 
DO 500 K = 1,4 

UCF(K) = SQRT((V+G4(K)*R*L)**2 + ( W-G4(K)*Q*L)**2) 

IF(UCF(K). GT. IE-10) THEN 

TERMO = (RH0/2)*(CDY*HH(K)*(V+G4(K)*R*L)**2 + 

& CDZ*BR(K) Vr ( W-G4(K) *q* L ) ** 2 ) 

TERM1 = TERMO- ( V+G4 ( K ) ,V R*L ) / UCF ( K ) 

TERM 2 = TERM0*(W-G4(K)*Q*L)/UCF(K) 

LATYAW = LATYAW + TERM1*GK4( K)*L 
NORPIT = NORPIT + TERM2*GK4(K)*L 
END IF 
500 CONTINUE 
C 

C FORCE EQUATIONS 
C 

c 

C LONGITUDINAL FORCE 
C 

FP( 1) = MASS--V--R - MASS*W*Q + MASS*XG*Q**2 + MASS*XG''R**2- 
MASS" YG-P*Q - MASS*ZG*P*R + (RH0/2)*L**4*(XPP*P**2 + 
XQQ*Q**2 + XRR"'R"'"2 + XPR*P*R) +(RH0/2)*L**3*(XWQ*W*Q + 
XVP“V"P+XVR'“V'“'R+U"Q*(XQDS ,V DS+XQDB ,,: 'DB)+XRDR*U"R' V DR) + 

( RHO/2 )*L" " 2" ( XVV*V**2 + XWW*W**2 + XVDR*U*V*DR + U*W* 
(X\\ , DS"'DS+XWDB"DB)+U**2' V (XDSDS*DS**2+XDBDB*DB"*2+ 
XDRDR"DR""2 ) ) - ( WEIGHT - BOY ) *S IN( THETA ) +( RHO/ 2 ) *L**3* 
XQDSN*U*Q*DS*EPS+( RHO/2) *l**2*( XWDSN*U*W*DS+XDSDSN*U**2- 
DS" "2) "EPS +(RH0/2)*L**2*U**2*XPR0P 
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c 



LATERAL FORCE 



FP( 2) = -MASS*U*R - MASS*XG*P*Q + MASS*YG*R**2 - MASS*ZG*Q*R + 

6< ( RHO/2 )*L**4*( YPQ*P*Q + YQR*Q*R)+(RHO/2)*L**3*(YP*U*P + 

6c YR*U*R + YVQ*V*Q + YWP*W*P + YWR*W*R) + ( RHO/2 )*L**2* 

6c (YV*U*V + YVW*V*W +YDR*U**2*DR) -LATYAW +( WEIGHT-BOY)* 

& COS ( THETA )*SIN( PH I ) +MAS S*W* P+MAS S* YG*P** 2 

NORMAL FORCE 

FP( 3) = MASS*U*Q - MASS*V*P - MASS*XG*P*R - MASS*YG*Q*R + 

& MASS*ZG*P**2 + MASS*ZG*Q**2 + ( RHO/2 )*L**4*(ZPP*P**2 + 

6c ZPR*P*R + ZRR*R**2) + ( RHO/2 )*L**3*( ZQ*U*Q + ZVP*V*P + 

& ZVR*V*R ) +( RHO/2 )*L**2*( ZW*U*W + ZVV*V**2 + U**2*(ZDS* 

6c DS+ZDB*DB) ) -NORPIT+( WEIGHT-BOY)*COS(THETA)*COS( PHI )+ 

6c ( RHO/2 )*L**3*ZQN*U*Q*EPS +(RHO/2)*L**2*( ZWN*U*W +ZDSN* 

6c U**2*DS)*EPS 

ROLL FORCE 

FP(4) = - I Z*Q*R +IY*Q*R -IXY*P*R +IYZ*Q**2 -IYZ*R**2 +IXZ*P*Q + 
6c MASS*YG*U*Q -MASS*YG*V*P -MASS*ZG*W*P+( RHO/2 )*L**5*( KPQ* 

6c P*Q + KQR*Q*R) +(RH0/2)*L**4*(KP*U*P +KR*U*R + KVQ*V*Q + 

6c KWP*W*P + KWR*W*R ) +( RHO/2 )*L**3*(KV*U*V + KW*V*W) + 

6c (YG*WEIGHT - YB*BOY ) *COS ( THETA ) *COS ( PHI ) - ( ZG*WE I GHT - 

6c ZB*BOY)*COS(THETA)*SIN( PHI ) + ( RHO/2 )*L**4*KPN*U*P*EPS+ 

6c (RHO/2 )*l**3*U**2*KPROP +MASS*ZG*U*R 

PITCH FORCE 

FP( 5 ) = - I X*P*R +IZ*P*R +IXY*Q*R -IYZ*P*Q -IXZ*P**2 +IXZ*R**2 - 
6c MASS*XG*U*Q + MASS*XG*V*P + MASS*ZG*V*R - MASS*ZG*W*Q + 

6c ( RHO/2 )*L**5*(MPP*P**2 +MPR*P*R +MRR*R**2 )+( RHO/2 )*L**4* 

6c ( MQ*U*Q + MVP*V*P + MVR*V*R ) + (RHO/2)*L**3*(MW*U*W + 

6c MVV*V**2+U**2*(MDS*DS+MDB*DB) )+ NORPIT -(XG*WEIGHT- 

6c X B*B OY ) * CO S ( THETA ) *C 0 S ( PH I )+( RHO/2) *L**4*MQN*U*Q*E PS + 

6c (RHO/2 ) *L**3*( MWN* U*W +MD SN*U** 2 *D S ) *EPS - 

6c ( ZG*WE I GHT - ZB*BOY ) * S I N( THETA ) 

YAW FORCE 

FP( 6) = -IY*P*Q +IX*P*Q +IXY*P**2 -IXY*Q**2 +IYZ*P*R -IXZ*Q*R - 
6c MASS*XG*U*R + MASS*XG*W*P - MASS*YG*V*R + MASS*YG*W*Q + 

6c ( RHO/2 )*L**5*( NPQ*P*Q + NQR*Q*R) +(RHO/2)*L**4*(NP*U*P+ 

6c NR*U*R + NVQ*V*Q +NWP*W*P + NWR*W*R) +(RHO/2)*L**3*(NV* 

6c U*V + NVW*Y*W + NDR* U** 2 *DR ) - LATYAW + ( XG*WE I GHT - 

6c XB*BOY)*COS( THETA )*SIN( PHI )+( YG*WEIGHT)*SIN(THETA) 

& +( RHO/2 )*L**3*U**2*NPROP-YB*BOY*SIN( THETA) 



NOW COMPUTE THE F(l-6) FUNCTIONS 

DO 600 J = 1,6 

F(J) = 0. 0 
DO 600 K = 1,6 
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nnnon onno 



F(J) = XMMINV(J,K)*FP(K) + F(J) 

600 CONTINUE 

THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 
FIRST SET THE DRIFT CURRENT VALUES 



UCO = 0. 0 
VCO = 0.0 
WCO = 0.0 

INERTIAL POSITION RATES F( 7-9) 

F( 7 ) = UCO + U*C0S( PSI )*C0S(THETA) + V*(COS(PSI)*SIN(THETA)* 

& SIN(PHI) - SIN( PSI )*COS( PHI ) ) + W*(COS(PSI)*SIN(THETA)* 

& COS(PHI) + SIN( PSI )*SIN( PHI) ) 

F( 8) = VCO + U*SIN( PSI )*COS(THETA) + V*(SIN(PSI)*SIN(THETA)* 

& SIN(PHI) + COS( PSI )*COS( PHI ) ) + W*( SIN( PSI)*SIN(THETA)* 

& COS(PHI) - COS( PSI )*SIN( PHI ) ) 

F( 9) = WCO - U*S IN (THETA) +V*COS ( THETA )*SIN( PHI) +W*COS( THETA)* 
& COS(PHI) 

EULER ANGLE RATES F( 10-12) 

F( 10) = P + Q*SIN( PHI )*TAN( THETA) + R*COS( PHI )*TAN( THETA) 

F( 11) = Q*COS( PHI ) - R*SIN( PHI ) 

F( 12) = Q*SIN( PHI )/COS( THETA) + R*COS( PHI) /COS (THETA) 



UDOT = F(l) 

VDOT = F(2) 

WDOT = F(3) 

PDOT = F(4) 

QDOT = F(5) 

RDOT = F(6) 

XDOT = F(7) 

YDOT = F(8) 

ZDOT = F(9) 

PHIDOT = F( 10) 

THETAD = F( 11) 

PSIDOT = F( 12 ) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
CREATE OUTPUT DATA FILE 



TESTER=MOD(FLOAT( I) , 100. ) 

IF (TESTER .EQ. 0.0) THEN 
TIMER=FLOAT( I)/2. 

WRITE (8,730) DS , DR ,XPOS ,YPOS , ZPOS , ROLL, PITCH, YAW 
WRITE (8,730) FP(5) 

WRITE (8,730) U,V,W,P,Q,R 
WRITE (8,730) IX, IZ , IXY , IYZ , IXZ 
WRITE (8,730) MASS,XG,ZG 
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C WRITE (8,730) RHO , L , MPP , MPR , MRR 

C WRITE (8,730) MQ , MVP , MVR , MW 

C WRITE (8,730) MW, MDS ,DS ,MDB ,DB ,NORPIT, WEIGHT 

C WRITE (8,730) XB , BOY, THETA, PHI ,MQN, EPS 

C WRITE (8,730) MWN,MDSN,ZB 

C WRITE (8,730) TIME , DEPTH ,DS , THETA , S 

C20 FORMAT ( IX ,' TIME ', 3X ' COMMAND DEPTH' , 3X, ' DIVE P ANG' , 

C *3X, 'PITCH ANGLE’ ,3X, SLIDING SURFACE'/) 

FORMAT (1X.F6. 2,2X,E11. 3 3X,E11. 3 ,4X,E10. 3,3X,E11. 3) 
FORMAT (IX,' TIME ' , 3X, U',3X,' ETA' ,3X, XPROP’ , 

*3X, 'DS') 

WRITE (8,730) TIME, U, ETA, XPROP, DS 
FORMAT (1X.F6. 2,2X,F6. 2 , 2X ,F6. 3 , 2X,F8. 4 , 2X.E11. 3) 

END IF 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C 
C 



C30 

710 



730 

C 



c 

c 

c 

C 

C 

c 

c 

c 

c 

c 

c 

c 



FIRST ORDER INTEGRATION 
U = U + DELT*UDOT 
V = V + DELT*VDOT 
W = W + DELT-'-WDOT 
P = P + DELT*PDOT 
Q = Q + DELT*QDOT 
R = R + DELT*RDOT 
XPOS = XPOS + DELT*XDOT 
YPOS = YPOS + DELT-YDOT 
ZPOS = ZPOS + DELT*ZDOT 
PHI = PHI + DELT*PHIDOT 
THETA = THETA + DELT*THETAD 
PS I = PS I + DELT*PSIDOT 



U = SURGE RATE 

V = SWAY RATE 
W = HEAVE RATE 

P = ROLL RATE 
Q = PITCH RATE 
R = YAW RATE 
X = SURGE 

Y = SWAY 
Z = HEAVE 
PHI = ROLL 
THETA = PITCH 
PS I = YAW 



C 

C 

C 

C 

C 



********** OBSERVER OF SLIDING CONTROLLER *********** 

CALL OB SER ( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT ,DELT,ZPOS,DS,UO) 



*********** SLIDING MODE CONTROL INPUT 
S=QHAT+0. 75*THAT-0. 0233*( ZHAT-COMZ) 

IF (ABS(S) . LT. BAR) SAT=(S/BAR)*ABS(S) 

IF (S . LE. -BAR) SAT=- 1 
IF (S . GE. BAR) SAT=1 
UHAT=1. 4286-QHAT+3. 1429*THAT 
UB AR=E I T A* S AT 
DE=UHAT+UBAR 
IF (DE .GE. 0.4) DS=0. 4 
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IF (DE . LT. -0. 4) DS=-0. 4 

IF( (DE .LT. 0.4) .AND. (DE . GE. -0. 4) ) DS=DE 
DB=-DS*1. 0 
TIME=TIME+DELT 
C 

PHIANG = PHI/0.0174532925 
THEANG = THETA/0.0174532925 
PSIANG = PSI/0. 0174532925 
C 

TRAC=-YPOS 

ROLL=PHIANG 

YAV=PSIANG 

DEPTH=-ZPOS 

DEPTH=ZPOS 

PITCH=THEANG 

BOWANG=(DB/. 01745) 

STNANG=(DS/. 01745) 



100 CONTINUE 
STOP 
END 

C ******** OBSERVER SUBROUTINE ******* 

SUBROUTINE OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , DELT , 
*ZPOS ,DS ,U) 

C 

QHAD0T=-0. 7*QHAT-0. 03*THAT-0. 035*DS-20. 9293*( ZPOS-ZHAT) 

TH ADOT=QHAT - 1 4 . 4 0 9 2* ( Z P 0 S - Z HAT ) 

ZHADOT= - 6*THAT+ 1 6 . 45*( Z POS - ZHAT ) 

C 

QHAT=QHAT+DELT*QHADOT 
TH AT=TH AT+D E LT*TH AD OT 
Z HAT=ZHAT+DE LT* ZHADOT 
RETURN 
END 
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